<commit_before>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <tf/transform_listener.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <geometry_msgs/PointStamped.h>

#include <string>
#include <math.h>
#include <vector>

#include "dmath/geometry.h"

class ArtificialPotentialField{
public:
    ArtificialPotentialField(ros::NodeHandle &node) :
        base_link_("base_link"),
        cmd_pub_(node.advertise<geometry_msgs::Twist>("cmd_vel", 10)),
        obs_sub_(node.subscribe("octomap_full", 10, &ArtificialPotentialField::obstacleCallback, this)),
        goal_sub_(node.subscribe("clicked_point", 10, &ArtificialPotentialField::goalCallback, this))

    {
        collision_map_.header.stamp = ros::Time(0);
    }

    void spin(){
        ros::Rate r(10);

        ros::Duration(1).sleep();
        geometry_msgs::Twist cmd;
        cmd.linear.z = 0.15;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        cmd.linear.z = 0;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        const double force = 0.025;

        while(ros::ok()){
            if(collision_map_.header.stamp != ros::Time(0)){
                std::vector<dmath::Vector3D> obstacles_lc;
                std::string map_frame = collision_map_.header.frame_id;
                octomap::OcTree *tree = dynamic_cast<octomap::OcTree*>(octomap_msgs::msgToMap(collision_map_));
                octomap::OcTree::leaf_iterator const end_it = tree->end_leafs();
                for(octomap::OcTree::leaf_iterator it = tree->begin_leafs(0); it != end_it; it++){
                    geometry_msgs::PointStamped p_in, p_out;
                    p_in.header.frame_id = map_frame;
                    p_in.point.x = it.getX();
                    p_in.point.y = it.getY();
                    p_in.point.z = it.getZ();

                    try{
                        tf_listener_.transformPoint(base_link_, p_in, p_out);
                        dmath::Vector3D obs(p_out.point.x, p_out.point.y, p_out.point.z);
                        if(magnitude(obs) < 2.0){
                            obstacles_lc.push_back(obs);
                        }
                    }catch(tf::TransformException &ex){
                        ROS_ERROR_STREAM("Exception trying to transform octomap: " << ex.what());
                    }
                }

                dmath::Vector3D Fs;
                for(int i=0; i < obstacles_lc.size(); i++){
                    Fs += get_potential_force(obstacles_lc[i], 0, 0.3, 1.0, 1.0);
                }

                geometry_msgs::PointStamped goal_msg_lc;
                dmath::Vector3D goal_lc;
                try{
                    tf_listener_.waitForTransform(goal_msg_gl_.header.frame_id, base_link_, ros::Time(0), ros::Duration(1));
                    goal_msg_gl_.header.stamp = ros::Time(0);
                    tf_listener_.transformPoint(base_link_, goal_msg_gl_, goal_msg_lc);
                    goal_lc = -dmath::Vector3D(goal_msg_lc.point.x, goal_msg_lc.point.y, goal_msg_lc.point.z);
                }catch(tf::TransformException &ex){
                    ROS_ERROR_STREAM("Exception trying to transform goal position: " << ex.what());
                    goal_lc = dmath::Vector3D();
                }

                Fs += get_potential_force(goal_lc, 100, 0, 1, 1);

                dmath::Vector3D vel = Fs * force;
                if(vel.x > 0.5) vel.x = 0.5;
                if(vel.x < -0.5) vel.x = -0.5;
                cmd.linear.x = vel.x;
                cmd.linear.y = vel.y;
                cmd.linear.z = vel.z;

                cmd_pub_.publish(cmd);
            }
            r.sleep();
            ros::spinOnce();
        }
    }

private:
    dmath::Vector3D get_potential_force(const dmath::Vector3D &dest_lc, double A = 1, double B = 1, double n = 1, double m = 1){
        dmath::Vector3D u = dest_lc;
        u = normalize(u);

        const double d = magnitude(dest_lc);
        ROS_INFO_STREAM("dist = " << d);
        double U = 0;
        if(fabs(d) > dmath::tol){
            U = -A/pow(d, n) + B/pow(d, m);
        }

        return U * u;
    }

    void obstacleCallback(const octomap_msgs::OctomapPtr &obs_msg){
        collision_map_ = *obs_msg;
    }

    void goalCallback(const geometry_msgs::PointStamped &goal_msg){
        goal_msg_gl_ = goal_msg;
    }

    octomap_msgs::Octomap collision_map_;
    ros::Publisher cmd_pub_;
    ros::Subscriber obs_sub_, goal_sub_;
    tf::TransformListener tf_listener_;
    std::string base_link_;
    geometry_msgs::PointStamped goal_msg_gl_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, "apf_planner");

    ros::NodeHandle node;
    ArtificialPotentialField apf(node);
    apf.spin();

    return 0;
}

<commit_msg>Fix obstacle direction<commit_after>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <tf/transform_listener.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <geometry_msgs/PointStamped.h>

#include <string>
#include <math.h>
#include <vector>

#include "dmath/geometry.h"

class ArtificialPotentialField{
public:
    ArtificialPotentialField(ros::NodeHandle &node) :
        base_link_("base_link"),
        cmd_pub_(node.advertise<geometry_msgs::Twist>("cmd_vel", 10)),
        obs_sub_(node.subscribe("octomap_full", 10, &ArtificialPotentialField::obstacleCallback, this)),
        goal_sub_(node.subscribe("clicked_point", 10, &ArtificialPotentialField::goalCallback, this))

    {
        collision_map_.header.stamp = ros::Time(0);
    }

    void spin(){
        ros::Rate r(10);

        ros::Duration(1).sleep();
        geometry_msgs::Twist cmd;
        cmd.linear.z = 0.15;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        cmd.linear.z = 0;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        const double force = 0.025;

        while(ros::ok()){
            if(collision_map_.header.stamp != ros::Time(0)){
                std::vector<dmath::Vector3D> obstacles_lc;
                std::string map_frame = collision_map_.header.frame_id;
                octomap::OcTree *tree = dynamic_cast<octomap::OcTree*>(octomap_msgs::msgToMap(collision_map_));
                octomap::OcTree::leaf_iterator const end_it = tree->end_leafs();
                for(octomap::OcTree::leaf_iterator it = tree->begin_leafs(0); it != end_it; it++){
                    geometry_msgs::PointStamped p_in, p_out;
                    p_in.header.frame_id = map_frame;
                    p_in.point.x = it.getX();
                    p_in.point.y = it.getY();
                    p_in.point.z = it.getZ();

                    try{
                        tf_listener_.transformPoint(base_link_, p_in, p_out);
                        dmath::Vector3D obs(p_out.point.x, p_out.point.y, p_out.point.z);
                        if(magnitude(obs) < 2.0){
                            obstacles_lc.push_back(-obs);
                        }
                    }catch(tf::TransformException &ex){
                        ROS_ERROR_STREAM("Exception trying to transform octomap: " << ex.what());
                    }
                }

                dmath::Vector3D Fs;
                for(int i=0; i < obstacles_lc.size(); i++){
                    Fs += get_potential_force(obstacles_lc[i], 0, 0.3, 1.0, 1.0);
                }

                geometry_msgs::PointStamped goal_msg_lc;
                dmath::Vector3D goal_lc;
                try{
                    tf_listener_.waitForTransform(goal_msg_gl_.header.frame_id, base_link_, ros::Time(0), ros::Duration(1));
                    goal_msg_gl_.header.stamp = ros::Time(0);
                    tf_listener_.transformPoint(base_link_, goal_msg_gl_, goal_msg_lc);
                    goal_lc = -dmath::Vector3D(goal_msg_lc.point.x, goal_msg_lc.point.y, goal_msg_lc.point.z);
                }catch(tf::TransformException &ex){
                    ROS_ERROR_STREAM("Exception trying to transform goal position: " << ex.what());
                    goal_lc = dmath::Vector3D();
                }

                Fs += get_potential_force(goal_lc, 100, 0, 1, 1);

                dmath::Vector3D vel = Fs * force;
                if(vel.x > 0.5) vel.x = 0.5;
                if(vel.x < -0.5) vel.x = -0.5;
                cmd.linear.x = vel.x;
                cmd.linear.y = vel.y;
                cmd.linear.z = vel.z;

                cmd_pub_.publish(cmd);
            }
            r.sleep();
            ros::spinOnce();
        }
    }

private:
    dmath::Vector3D get_potential_force(const dmath::Vector3D &dest_lc, double A = 1, double B = 1, double n = 1, double m = 1){
        dmath::Vector3D u = dest_lc;
        u = normalize(u);

        const double d = magnitude(dest_lc);
        ROS_INFO_STREAM("dist = " << d);
        double U = 0;
        if(fabs(d) > dmath::tol){
            U = -A/pow(d, n) + B/pow(d, m);
        }

        return U * u;
    }

    void obstacleCallback(const octomap_msgs::OctomapPtr &obs_msg){
        collision_map_ = *obs_msg;
    }

    void goalCallback(const geometry_msgs::PointStamped &goal_msg){
        goal_msg_gl_ = goal_msg;
    }

    octomap_msgs::Octomap collision_map_;
    ros::Publisher cmd_pub_;
    ros::Subscriber obs_sub_, goal_sub_;
    tf::TransformListener tf_listener_;
    std::string base_link_;
    geometry_msgs::PointStamped goal_msg_gl_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, "apf_planner");

    ros::NodeHandle node;
    ArtificialPotentialField apf(node);
    apf.spin();

    return 0;
}

<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: xmlImage.cxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: rt $ $Date: 2007-07-09 11:56:18 $
 *
 *  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
 *
 ************************************************************************/
#include "precompiled_reportdesign.hxx"

#ifndef RPT_XMLIMAGE_HXX
#include "xmlImage.hxx"
#endif
#ifndef RPT_XMLFILTER_HXX
#include "xmlfilter.hxx"
#endif
#ifndef _XMLOFF_XMLTOKEN_HXX
#include <xmloff/xmltoken.hxx>
#endif
#ifndef _XMLOFF_XMLNMSPE_HXX
#include <xmloff/xmlnmspe.hxx>
#endif
#ifndef _XMLOFF_NMSPMAP_HXX
#include <xmloff/nmspmap.hxx>
#endif
#ifndef RPT_XMLENUMS_HXX
#include "xmlEnums.hxx"
#endif
#ifndef RPT_XMLCOMPONENT_HXX
#include "xmlComponent.hxx"
#endif
#ifndef RPT_XMLREPORTELEMENT_HXX
#include "xmlReportElement.hxx"
#endif
#ifndef RPT_XMLCONTROLPROPERTY_HXX
#include "xmlControlProperty.hxx"
#endif
#ifndef _TOOLS_DEBUG_HXX
#include <tools/debug.hxx>
#endif

namespace rptxml
{
    using namespace ::rtl;
    using namespace ::com::sun::star;
    using namespace ::com::sun::star::uno;
    using namespace ::com::sun::star::xml::sax;
DBG_NAME( rpt_OXMLImage )

OXMLImage::OXMLImage( ORptFilter& rImport,
                sal_uInt16 nPrfx, const OUString& rLName,
                const Reference< XAttributeList > & _xAttrList
                ,const Reference< XImageControl > & _xComponent
                ,OXMLTable* _pContainer) :
    OXMLReportElementBase( rImport, nPrfx, rLName,_xComponent.get(),_pContainer)
{
    DBG_CTOR( rpt_OXMLImage,NULL);

    OSL_ENSURE(m_xComponent.is(),"Component is NULL!");
    const SvXMLNamespaceMap& rMap = m_rImport.GetNamespaceMap();
    const SvXMLTokenMap& rTokenMap = m_rImport.GetControlElemTokenMap();
    static const ::rtl::OUString s_sTRUE = ::xmloff::token::GetXMLToken(XML_TRUE);

    const sal_Int16 nLength = (_xAttrList.is()) ? _xAttrList->getLength() : 0;
    try
    {
        for(sal_Int16 i = 0; i < nLength; ++i)
        {
            OUString sLocalName;
            const rtl::OUString sAttrName = _xAttrList->getNameByIndex( i );
            const sal_uInt16 nPrefix = rMap.GetKeyByAttrName( sAttrName,&sLocalName );
            rtl::OUString sValue = _xAttrList->getValueByIndex( i );

            switch( rTokenMap.Get( nPrefix, sLocalName ) )
            {
                case XML_TOK_IMAGE_DATA:
                    _xComponent->setImageURL(rImport.GetAbsoluteReference( sValue ));
                    break;
                case XML_TOK_PRESERVE_IRI:
                    _xComponent->setPreserveIRI(s_sTRUE == sValue);
                    break;
                case XML_TOK_SCALE:
                    _xComponent->setScaleImage(s_sTRUE == sValue);
                    break;
                case XML_TOK_DATA_FORMULA:
                    {
                        sal_Int32 nLen = sValue.getLength();
                        if ( nLen )
                        {
                            sValue = sValue.copy(5,nLen-6);
                            _xComponent->setDataField(sValue);
                        }
                    }
                    break;
                default:
                    break;
            }
        }
    }
    catch(Exception&)
    {
        OSL_ENSURE(0,"Exception catched while filling the image props");
    }
}
// -----------------------------------------------------------------------------

OXMLImage::~OXMLImage()
{

    DBG_DTOR( rpt_OXMLImage,NULL);
}
// -----------------------------------------------------------------------------

//----------------------------------------------------------------------------
} // namespace rptxml
// -----------------------------------------------------------------------------
<commit_msg>INTEGRATION: CWS rpt23fix02 (1.2.4); FILE MERGED 2007/07/30 06:47:32 oj 1.2.4.2: #i79862# data field not correctly set 2007/07/27 12:21:58 oj 1.2.4.1: #i77907# remove entries from context when propbrw is destroyed<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: xmlImage.cxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: hr $ $Date: 2007-08-03 09:57:31 $
 *
 *  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
 *
 ************************************************************************/
#include "precompiled_reportdesign.hxx"

#ifndef RPT_XMLIMAGE_HXX
#include "xmlImage.hxx"
#endif
#ifndef RPT_XMLFILTER_HXX
#include "xmlfilter.hxx"
#endif
#ifndef _XMLOFF_XMLTOKEN_HXX
#include <xmloff/xmltoken.hxx>
#endif
#ifndef _XMLOFF_XMLNMSPE_HXX
#include <xmloff/xmlnmspe.hxx>
#endif
#ifndef _XMLOFF_NMSPMAP_HXX
#include <xmloff/nmspmap.hxx>
#endif
#ifndef RPT_XMLENUMS_HXX
#include "xmlEnums.hxx"
#endif
#ifndef RPT_XMLCOMPONENT_HXX
#include "xmlComponent.hxx"
#endif
#ifndef RPT_XMLREPORTELEMENT_HXX
#include "xmlReportElement.hxx"
#endif
#ifndef RPT_XMLCONTROLPROPERTY_HXX
#include "xmlControlProperty.hxx"
#endif
#ifndef _TOOLS_DEBUG_HXX
#include <tools/debug.hxx>
#endif

namespace rptxml
{
    using namespace ::rtl;
    using namespace ::com::sun::star;
    using namespace ::com::sun::star::uno;
    using namespace ::com::sun::star::xml::sax;
DBG_NAME( rpt_OXMLImage )

OXMLImage::OXMLImage( ORptFilter& rImport,
                sal_uInt16 nPrfx, const OUString& rLName,
                const Reference< XAttributeList > & _xAttrList
                ,const Reference< XImageControl > & _xComponent
                ,OXMLTable* _pContainer) :
    OXMLReportElementBase( rImport, nPrfx, rLName,_xComponent.get(),_pContainer)
{
    DBG_CTOR( rpt_OXMLImage,NULL);

    OSL_ENSURE(m_xComponent.is(),"Component is NULL!");
    const SvXMLNamespaceMap& rMap = m_rImport.GetNamespaceMap();
    const SvXMLTokenMap& rTokenMap = m_rImport.GetControlElemTokenMap();
    static const ::rtl::OUString s_sTRUE = ::xmloff::token::GetXMLToken(XML_TRUE);

    const sal_Int16 nLength = (_xAttrList.is()) ? _xAttrList->getLength() : 0;
    try
    {
        for(sal_Int16 i = 0; i < nLength; ++i)
        {
            OUString sLocalName;
            const rtl::OUString sAttrName = _xAttrList->getNameByIndex( i );
            const sal_uInt16 nPrefix = rMap.GetKeyByAttrName( sAttrName,&sLocalName );
            rtl::OUString sValue = _xAttrList->getValueByIndex( i );

            switch( rTokenMap.Get( nPrefix, sLocalName ) )
            {
                case XML_TOK_IMAGE_DATA:
                    _xComponent->setImageURL(rImport.GetAbsoluteReference( sValue ));
                    break;
                case XML_TOK_PRESERVE_IRI:
                    _xComponent->setPreserveIRI(s_sTRUE == sValue);
                    break;
                case XML_TOK_SCALE:
                    _xComponent->setScaleImage(s_sTRUE == sValue);
                    break;
                case XML_TOK_DATA_FORMULA:
                    _xComponent->setDataField(ORptFilter::convertFormula(sValue));
                    break;
                default:
                    break;
            }
        }
    }
    catch(Exception&)
    {
        OSL_ENSURE(0,"Exception catched while filling the image props");
    }
}
// -----------------------------------------------------------------------------

OXMLImage::~OXMLImage()
{

    DBG_DTOR( rpt_OXMLImage,NULL);
}
// -----------------------------------------------------------------------------

//----------------------------------------------------------------------------
} // namespace rptxml
// -----------------------------------------------------------------------------
<|endoftext|>


<commit_before>#include <cstdio>
#include <memory>

class A {
 private:
  int id;

 public:
  A(int id): id(id) {
    printf("constructed %d\n", this->id);
  }
  ~A() {
    printf("destructed %d\n", this->id);
  }
};

int main(void) {
  std::unique_ptr<A> a = std::unique_ptr<A>(new A(0));

  std::unique_ptr<A> b = nullptr;
  printf("%p %s\n", a.get(), a ? "true" : "false");
  printf("%p %s\n", b.get(), b ? "true" : "false");

  b = std::unique_ptr<A>(new A(1));

  a = std::unique_ptr<A>(new A(2));
  return 0;
}
<commit_msg>[C++] unique_ptr.reset()<commit_after>#include <cstdio>
#include <memory>

class A {
 private:
  int id;

 public:
  A(int id): id(id) {
    printf("constructed %d\n", this->id);
  }
  ~A() {
    printf("destructed %d\n", this->id);
  }
};

int main(void) {
  std::unique_ptr<A> a = std::unique_ptr<A>(new A(0));

  std::unique_ptr<A> b = nullptr;
  printf("%p %s\n", a.get(), a ? "true" : "false");
  printf("%p %s\n", b.get(), b ? "true" : "false");

  b.reset(new A(1));

  a = std::unique_ptr<A>(new A(2));
  b.reset(new A(3));
  return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2016 ScyllaDB
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "mutation_partition.hh"
#include "streamed_mutation.hh"
#include "utils/anchorless_list.hh"
#include "utils/logalloc.hh"

// This is MVCC implementation for mutation_partitions.
//
// It is assumed that mutation_partitions are stored in some sort of LSA-managed
// container (memtable or row cache).
//
// partition_entry - the main handle to the mutation_partition, allows writes
//                   and reads.
// partition_version - mutation_partition inside a list of partition versions.
//                     mutation_partition represents just a difference against
//                     the next one in the list. To get a single
//                     mutation_partition fully representing this version one
//                     needs to merge this one and all its successors in the
//                     list.
// partition_snapshot - a handle to some particular partition_version. It allows
//                      only reads and itself is immutable the partition version
//                      it represents won't be modified as long as the snapshot
//                      is alive.
//
// pe - partition_entry
// pv - partition_version
// ps - partition_snapshot
// ps(u) - partition_snapshot marked as unique owner

// Scene I. Write-only loads
//   pv
//   ^
//   |
//   pe
// In case of write-only loads all incoming mutations are directly applied
// to the partition_version that partition_entry is pointing to. The list
// of partition_versions contains only a single element.
//
// Scene II. Read-only loads
//   pv
//   ^
//   |
//   pe <- ps
// In case of read-only scenarios there is only a single partition_snapshot
// object that points to the partition_entry. There is only a single
// partition_version.
//
// Scene III. Writes and reads
//   pv -- pv -- pv
//   ^     ^     ^
//   |     |     |
//   pe    ps    ps
// If the partition_entry that needs to be modified is currently read from (i.e.
// there exist a partition_snapshot pointing to it) instead of applying new
// mutation directly a new partition version is created and added at the front
// of the list. partition_entry points to the new version (so that it has the
// most recent view of stored data) while the partition_snapshot points to the
// same partition_version it pointed to before (so that the data it sees doesn't
// change).
// As a result the list may contain multiple partition versions used by
// different partition snapshots.
// When the partition_snapshot is destroyed partition_versions are squashed
// together to minimize the amount of elements on the list.
//
// Scene IV. Schema upgrade
//   pv    pv --- pv
//   ^     ^      ^
//   |     |      |
//   pe    ps(u)  ps
// When there is a schema upgrade the list of partition versions pointed to
// by partition_entry is replaced by a new single partition_version that is a
// result of squashing and upgrading the old versions.
// Old versions not used by any partition snapshot are removed. The first
// partition snapshot on the list is marked as unique which means that upon
// its destruction it won't attempt to squash versions but instead remove
// the unused ones and pass the "unique owner" mark the next snapshot on the
// list (if there is any).
//
// Scene V. partition_entry eviction
//   pv
//   ^
//   |
//   ps(u)
// When partition_entry is removed (e.g. because it was evicted from cache)
// the partition versions are removed in a similar manner than in the schema
// upgrade scenario. The unused ones are destroyed right away and the first
// snapshot on the list is marked as unique owner so that on its destruction
// it continues removal of the partition versions.

class partition_version_ref;

class partition_version : public anchorless_list_base_hook<partition_version> {
    partition_version_ref* _backref = nullptr;
    mutation_partition _partition;

    friend class partition_version_ref;
public:
    explicit partition_version(schema_ptr s) noexcept
        : _partition(std::move(s)) { }
    explicit partition_version(mutation_partition mp) noexcept
        : _partition(std::move(mp)) { }
    partition_version(partition_version&& pv) noexcept;
    partition_version& operator=(partition_version&& pv) noexcept;
    ~partition_version();

    mutation_partition& partition() { return _partition; }
    const mutation_partition& partition() const { return _partition; }

    bool is_referenced() { return _backref; }
    partition_version_ref& back_reference() { return *_backref; }
};

class partition_version_ref {
    partition_version* _version = nullptr;
    bool _unique_owner = false;

    friend class partition_version;
public:
    partition_version_ref() = default;
    explicit partition_version_ref(partition_version& pv) noexcept : _version(&pv) {
        assert(!_version->_backref);
        _version->_backref = this;
    }
    ~partition_version_ref() {
        if (_version) {
            _version->_backref = nullptr;
        }
    }
    partition_version_ref(partition_version_ref&& other) noexcept : _version(other._version) {
        if (_version) {
            _version->_backref = this;
        }
        other._version = nullptr;
    }
    partition_version_ref& operator=(partition_version_ref&& other) noexcept {
        if (this != &other) {
            this->~partition_version_ref();
            new (this) partition_version_ref(std::move(other));
        }
        return *this;
    }

    explicit operator bool() { return _version; }

    partition_version& operator*() {
        assert(_version);
        return *_version;
    }
    partition_version* operator->() {
        assert(_version);
        return _version;
    }

    bool is_unique_owner() const { return _unique_owner; }
    void mark_as_unique_owner() { _unique_owner = true; }
};

class partition_entry;

class partition_snapshot : public enable_lw_shared_from_this<partition_snapshot> {
public:
    // Only snapshots created with the same value of phase can point to the same version.
    using phase_type = uint64_t;
    static constexpr phase_type default_phase = 0;
    static constexpr phase_type max_phase = std::numeric_limits<phase_type>::max();
private:
    schema_ptr _schema;
    // Either _version or _entry is non-null.
    partition_version_ref _version;
    partition_entry* _entry;
    phase_type _phase;

    friend class partition_entry;
public:
    explicit partition_snapshot(schema_ptr s,
                                partition_entry* entry,
                                phase_type phase = default_phase)
        : _schema(std::move(s)), _entry(entry), _phase(phase) { }
    partition_snapshot(const partition_snapshot&) = delete;
    partition_snapshot(partition_snapshot&&) = delete;
    partition_snapshot& operator=(const partition_snapshot&) = delete;
    partition_snapshot& operator=(partition_snapshot&&) = delete;

    // If possible merges the version pointed to by this snapshot with
    // adjacent partition versions. Leaves the snapshot in an unspecified state.
    // Can be retried if previous merge attempt has failed.
    void merge_partition_versions();

    ~partition_snapshot();

    partition_version_ref& version();

    auto versions() {
        return version()->elements_from_this();
    }

    unsigned version_count();
};

class partition_entry {
    partition_snapshot* _snapshot = nullptr;
    partition_version_ref _version;

    friend class partition_snapshot;
    friend class cache_entry;
private:
    void set_version(partition_version*);

    void apply(const schema& s, partition_version* pv, const schema& pv_schema);
public:
    partition_entry() = default;
    explicit partition_entry(mutation_partition mp);
    ~partition_entry();

    partition_entry(partition_entry&& pe) noexcept
        : _snapshot(pe._snapshot), _version(std::move(pe._version))
    {
        if (_snapshot) {
            _snapshot->_entry = this;
        }
        pe._snapshot = nullptr;
    }
    partition_entry& operator=(partition_entry&& other) noexcept {
        if (this != &other) {
            this->~partition_entry();
            new (this) partition_entry(std::move(other));
        }
        return *this;
    }

    partition_version_ref& version() {
        return _version;
    }

    // Strong exception guarantees.
    // Assumes this instance and mp are fully continuous.
    void apply(const schema& s, const mutation_partition& mp, const schema& mp_schema);

    // Same exception guarantees as:
    // mutation_partition::apply(const schema&, mutation_partition&&, const schema&)
    void apply(const schema& s, mutation_partition&& mp, const schema& mp_schema);

    // Strong exception guarantees.
    // Assumes this instance and mpv are fully continuous.
    void apply(const schema& s, mutation_partition_view mpv, const schema& mp_schema);

    // Weak exception guarantees.
    // If an exception is thrown this and pe will be left in some valid states
    // such that if the operation is retried (possibly many times) and eventually
    // succeeds the result will be as if the first attempt didn't fail.
    void apply(const schema& s, partition_entry&& pe, const schema& pe_schema);

    // Ensures that the latest version can be populated with data from given phase
    // by inserting a new version if necessary.
    // Doesn't affect value or continuity of the partition.
    // Returns a reference to the new latest version.
    partition_version& open_version(const schema& s, partition_snapshot::phase_type phase = partition_snapshot::max_phase) {
        if (_snapshot && _snapshot->_phase != phase) {
            auto new_version = current_allocator().construct<partition_version>(mutation_partition(s.shared_from_this()));
            new_version->partition().set_static_row_continuous(_version->partition().static_row_continuous());
            new_version->insert_before(*_version);
            set_version(new_version);
            return *new_version;
        }
        return *_version;
    }

    mutation_partition squashed(schema_ptr from, schema_ptr to);

    // needs to be called with reclaiming disabled
    void upgrade(schema_ptr from, schema_ptr to);

    // Snapshots with different values of phase will point to different partition_version objects.
    lw_shared_ptr<partition_snapshot> read(schema_ptr entry_schema,
        partition_snapshot::phase_type phase = partition_snapshot::default_phase);
};

inline partition_version_ref& partition_snapshot::version()
{
    if (_version) {
        return _version;
    } else {
        return _entry->_version;
    }
}
<commit_msg>partition_version: Make operator bool() const-qualified<commit_after>/*
 * Copyright (C) 2016 ScyllaDB
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "mutation_partition.hh"
#include "streamed_mutation.hh"
#include "utils/anchorless_list.hh"
#include "utils/logalloc.hh"

// This is MVCC implementation for mutation_partitions.
//
// It is assumed that mutation_partitions are stored in some sort of LSA-managed
// container (memtable or row cache).
//
// partition_entry - the main handle to the mutation_partition, allows writes
//                   and reads.
// partition_version - mutation_partition inside a list of partition versions.
//                     mutation_partition represents just a difference against
//                     the next one in the list. To get a single
//                     mutation_partition fully representing this version one
//                     needs to merge this one and all its successors in the
//                     list.
// partition_snapshot - a handle to some particular partition_version. It allows
//                      only reads and itself is immutable the partition version
//                      it represents won't be modified as long as the snapshot
//                      is alive.
//
// pe - partition_entry
// pv - partition_version
// ps - partition_snapshot
// ps(u) - partition_snapshot marked as unique owner

// Scene I. Write-only loads
//   pv
//   ^
//   |
//   pe
// In case of write-only loads all incoming mutations are directly applied
// to the partition_version that partition_entry is pointing to. The list
// of partition_versions contains only a single element.
//
// Scene II. Read-only loads
//   pv
//   ^
//   |
//   pe <- ps
// In case of read-only scenarios there is only a single partition_snapshot
// object that points to the partition_entry. There is only a single
// partition_version.
//
// Scene III. Writes and reads
//   pv -- pv -- pv
//   ^     ^     ^
//   |     |     |
//   pe    ps    ps
// If the partition_entry that needs to be modified is currently read from (i.e.
// there exist a partition_snapshot pointing to it) instead of applying new
// mutation directly a new partition version is created and added at the front
// of the list. partition_entry points to the new version (so that it has the
// most recent view of stored data) while the partition_snapshot points to the
// same partition_version it pointed to before (so that the data it sees doesn't
// change).
// As a result the list may contain multiple partition versions used by
// different partition snapshots.
// When the partition_snapshot is destroyed partition_versions are squashed
// together to minimize the amount of elements on the list.
//
// Scene IV. Schema upgrade
//   pv    pv --- pv
//   ^     ^      ^
//   |     |      |
//   pe    ps(u)  ps
// When there is a schema upgrade the list of partition versions pointed to
// by partition_entry is replaced by a new single partition_version that is a
// result of squashing and upgrading the old versions.
// Old versions not used by any partition snapshot are removed. The first
// partition snapshot on the list is marked as unique which means that upon
// its destruction it won't attempt to squash versions but instead remove
// the unused ones and pass the "unique owner" mark the next snapshot on the
// list (if there is any).
//
// Scene V. partition_entry eviction
//   pv
//   ^
//   |
//   ps(u)
// When partition_entry is removed (e.g. because it was evicted from cache)
// the partition versions are removed in a similar manner than in the schema
// upgrade scenario. The unused ones are destroyed right away and the first
// snapshot on the list is marked as unique owner so that on its destruction
// it continues removal of the partition versions.

class partition_version_ref;

class partition_version : public anchorless_list_base_hook<partition_version> {
    partition_version_ref* _backref = nullptr;
    mutation_partition _partition;

    friend class partition_version_ref;
public:
    explicit partition_version(schema_ptr s) noexcept
        : _partition(std::move(s)) { }
    explicit partition_version(mutation_partition mp) noexcept
        : _partition(std::move(mp)) { }
    partition_version(partition_version&& pv) noexcept;
    partition_version& operator=(partition_version&& pv) noexcept;
    ~partition_version();

    mutation_partition& partition() { return _partition; }
    const mutation_partition& partition() const { return _partition; }

    bool is_referenced() { return _backref; }
    partition_version_ref& back_reference() { return *_backref; }
};

class partition_version_ref {
    partition_version* _version = nullptr;
    bool _unique_owner = false;

    friend class partition_version;
public:
    partition_version_ref() = default;
    explicit partition_version_ref(partition_version& pv) noexcept : _version(&pv) {
        assert(!_version->_backref);
        _version->_backref = this;
    }
    ~partition_version_ref() {
        if (_version) {
            _version->_backref = nullptr;
        }
    }
    partition_version_ref(partition_version_ref&& other) noexcept : _version(other._version) {
        if (_version) {
            _version->_backref = this;
        }
        other._version = nullptr;
    }
    partition_version_ref& operator=(partition_version_ref&& other) noexcept {
        if (this != &other) {
            this->~partition_version_ref();
            new (this) partition_version_ref(std::move(other));
        }
        return *this;
    }

    explicit operator bool() const { return _version; }

    partition_version& operator*() {
        assert(_version);
        return *_version;
    }
    partition_version* operator->() {
        assert(_version);
        return _version;
    }

    bool is_unique_owner() const { return _unique_owner; }
    void mark_as_unique_owner() { _unique_owner = true; }
};

class partition_entry;

class partition_snapshot : public enable_lw_shared_from_this<partition_snapshot> {
public:
    // Only snapshots created with the same value of phase can point to the same version.
    using phase_type = uint64_t;
    static constexpr phase_type default_phase = 0;
    static constexpr phase_type max_phase = std::numeric_limits<phase_type>::max();
private:
    schema_ptr _schema;
    // Either _version or _entry is non-null.
    partition_version_ref _version;
    partition_entry* _entry;
    phase_type _phase;

    friend class partition_entry;
public:
    explicit partition_snapshot(schema_ptr s,
                                partition_entry* entry,
                                phase_type phase = default_phase)
        : _schema(std::move(s)), _entry(entry), _phase(phase) { }
    partition_snapshot(const partition_snapshot&) = delete;
    partition_snapshot(partition_snapshot&&) = delete;
    partition_snapshot& operator=(const partition_snapshot&) = delete;
    partition_snapshot& operator=(partition_snapshot&&) = delete;

    // If possible merges the version pointed to by this snapshot with
    // adjacent partition versions. Leaves the snapshot in an unspecified state.
    // Can be retried if previous merge attempt has failed.
    void merge_partition_versions();

    ~partition_snapshot();

    partition_version_ref& version();

    auto versions() {
        return version()->elements_from_this();
    }

    unsigned version_count();
};

class partition_entry {
    partition_snapshot* _snapshot = nullptr;
    partition_version_ref _version;

    friend class partition_snapshot;
    friend class cache_entry;
private:
    void set_version(partition_version*);

    void apply(const schema& s, partition_version* pv, const schema& pv_schema);
public:
    partition_entry() = default;
    explicit partition_entry(mutation_partition mp);
    ~partition_entry();

    partition_entry(partition_entry&& pe) noexcept
        : _snapshot(pe._snapshot), _version(std::move(pe._version))
    {
        if (_snapshot) {
            _snapshot->_entry = this;
        }
        pe._snapshot = nullptr;
    }
    partition_entry& operator=(partition_entry&& other) noexcept {
        if (this != &other) {
            this->~partition_entry();
            new (this) partition_entry(std::move(other));
        }
        return *this;
    }

    partition_version_ref& version() {
        return _version;
    }

    // Strong exception guarantees.
    // Assumes this instance and mp are fully continuous.
    void apply(const schema& s, const mutation_partition& mp, const schema& mp_schema);

    // Same exception guarantees as:
    // mutation_partition::apply(const schema&, mutation_partition&&, const schema&)
    void apply(const schema& s, mutation_partition&& mp, const schema& mp_schema);

    // Strong exception guarantees.
    // Assumes this instance and mpv are fully continuous.
    void apply(const schema& s, mutation_partition_view mpv, const schema& mp_schema);

    // Weak exception guarantees.
    // If an exception is thrown this and pe will be left in some valid states
    // such that if the operation is retried (possibly many times) and eventually
    // succeeds the result will be as if the first attempt didn't fail.
    void apply(const schema& s, partition_entry&& pe, const schema& pe_schema);

    // Ensures that the latest version can be populated with data from given phase
    // by inserting a new version if necessary.
    // Doesn't affect value or continuity of the partition.
    // Returns a reference to the new latest version.
    partition_version& open_version(const schema& s, partition_snapshot::phase_type phase = partition_snapshot::max_phase) {
        if (_snapshot && _snapshot->_phase != phase) {
            auto new_version = current_allocator().construct<partition_version>(mutation_partition(s.shared_from_this()));
            new_version->partition().set_static_row_continuous(_version->partition().static_row_continuous());
            new_version->insert_before(*_version);
            set_version(new_version);
            return *new_version;
        }
        return *_version;
    }

    mutation_partition squashed(schema_ptr from, schema_ptr to);

    // needs to be called with reclaiming disabled
    void upgrade(schema_ptr from, schema_ptr to);

    // Snapshots with different values of phase will point to different partition_version objects.
    lw_shared_ptr<partition_snapshot> read(schema_ptr entry_schema,
        partition_snapshot::phase_type phase = partition_snapshot::default_phase);
};

inline partition_version_ref& partition_snapshot::version()
{
    if (_version) {
        return _version;
    } else {
        return _entry->_version;
    }
}
<|endoftext|>


<commit_before>/**
 * Copyright (C) 2017, Raffaello Bonghi <raffaello@rnext.it>
 * All rights reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "configurator/gpio_analog.h"

#define PARAM_ANALOG_STRING "/analog"

GPIOAnalogConfigurator::GPIOAnalogConfigurator(const ros::NodeHandle &nh, roboteq::serial_controller *serial, std::vector<roboteq::Motor *> motor, string name, unsigned int number)
    : nh_(nh)
    , mSerial(serial)
    ,_motor(motor)
{
    // Find path param
    mName = nh_.getNamespace() + name + PARAM_ANALOG_STRING + "/" + std::to_string(number);
    // Roboteq motor number
    mNumber = number;
    // Set false on first run
    setup_param = false;
}

void GPIOAnalogConfigurator::initConfigurator(bool load_from_board)
{
    // Check if is required load paramers
    if(load_from_board)
    {
        // Load parameters from roboteq
        getParamFromRoboteq();
    }
    // Initialize encoder dynamic reconfigure
    mDynRecServer = std::make_shared<ReconfigureServer>(mDynServerMutex, ros::NodeHandle(mName));
    // Load default configuration
    roboteq_control::RoboteqAnalogInputConfig config;
    mDynRecServer->getConfigDefault(config);
    // Update parameters
    mDynServerMutex.lock();
    mDynRecServer->updateConfig(config);
    mDynServerMutex.unlock();
    // Set callback
    mDynRecServer->setCallback(boost::bind(&GPIOAnalogConfigurator::reconfigureCBParam, this, _1, _2));
}

void GPIOAnalogConfigurator::getParamFromRoboteq()
{
    try
    {
        // conversion AMOD [pag. 292]
        string str_conversion = mSerial->getParam("AMOD", std::to_string(mNumber));
        int conversion = boost::lexical_cast<int>(str_conversion);
        // Set params
        nh_.setParam(mName + "/conversion", conversion);

        // input AINA [pag. 287]
        string str_pina = mSerial->getParam("AINA", std::to_string(mNumber));
        // Get AINA from roboteq board
        int emod = boost::lexical_cast<unsigned int>(str_pina);
        // 3 modes:
        // 0 - Unsed
        // 1 - Command
        // 2 - Feedback
        int command = (emod & 0b11);
        int motors = (emod - command) >> 4;
        int tmp1 = ((motors & 0b1) > 0);
        int tmp2 = ((motors & 0b10) > 0);
        if(tmp1)
        {
            for (vector<roboteq::Motor*>::iterator it = _motor.begin() ; it != _motor.end(); ++it)
            {
                roboteq::Motor* motor = ((roboteq::Motor*)(*it));
                if(motor->getNumber() == 1)
                {
                    motor->registerSensor(this);
                    ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
                    break;
                }
            }
        }
        if(tmp2)
        {
            for (vector<roboteq::Motor*>::iterator it = _motor.begin() ; it != _motor.end(); ++it)
            {
                roboteq::Motor* motor = ((roboteq::Motor*)(*it));
                if(motor->getNumber() == 2)
                {
                    motor->registerSensor(this);
                    ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
                    break;
                }
            }
        }

        // Set parameter
        nh_.setParam(mName + "/input_use", command);
        nh_.setParam(mName + "/input_motor_one", tmp1);
        nh_.setParam(mName + "/input_motor_two", tmp2);

        // polarity APOL [pag. 293]
        string str_polarity = mSerial->getParam("APOL", std::to_string(mNumber));
        int polarity = boost::lexical_cast<int>(str_polarity);
        // Set params
        nh_.setParam(mName + "/conversion_polarity", polarity);

        // Input deadband ADB [pag. 286]
        string str_deadband = mSerial->getParam("ADB", std::to_string(mNumber));
        int deadband = boost::lexical_cast<int>(str_deadband);
        // Set params
        nh_.setParam(mName + "/input_deadband", deadband);

        // Input AMIN [pag. 290]
        string str_min = mSerial->getParam("AMIN", std::to_string(mNumber));
        double min = boost::lexical_cast<double>(str_min) / 1000;
        // Set params
        nh_.setParam(mName + "/range_input_min", min);

        // Input AMAX [pag. 289]
        string str_max = mSerial->getParam("AMAX", std::to_string(mNumber));
        double max = boost::lexical_cast<double>(str_max) / 1000;
        // Set params
        nh_.setParam(mName + "/range_input_max", max);

        // Input ATCR [pag. 285]
        string str_ctr = mSerial->getParam("ACTR", std::to_string(mNumber));
        double ctr = boost::lexical_cast<double>(str_ctr) / 1000;
        // Set params
        nh_.setParam(mName + "/range_input_center", ctr);

    } catch (std::bad_cast& e)
    {
        ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what());
    }
}

void GPIOAnalogConfigurator::reconfigureCBParam(roboteq_control::RoboteqAnalogInputConfig &config, uint32_t level)
{
    //The first time we're called, we just want to make sure we have the
    //original configuration
    if(!setup_param)
    {
      _last_param_config = config;
      default_param_config = _last_param_config;
      setup_param = true;
      return;
    }

    if(config.restore_defaults)
    {
        //if someone sets restore defaults on the parameter server, prevent looping
        config.restore_defaults = false;
        // Overload config with default
        config = default_param_config;
    }

    if(config.load_roboteq)
    {
        //if someone sets again the request on the parameter server, prevent looping
        config.load_roboteq = false;
        // Launch param load
        getParamFromRoboteq();
        // Skip other read
        return;
    }

    // Set conversion AMOD [pag. 292]
    if(_last_param_config.conversion != config.conversion)
    {
        // Update operative mode
        mSerial->setParam("AMOD", std::to_string(mNumber) + " " + std::to_string(config.conversion));
    }
    // Set input AINA [pag. 287]
    if((_last_param_config.input_use != config.input_use) ||
            (_last_param_config.input_motor_one != config.input_motor_one) ||
            (_last_param_config.input_motor_two != config.input_motor_two))
    {
        int input = config.input_use + 16*config.input_motor_one + 32*config.input_motor_two;
        mSerial->setParam("AINA", std::to_string(mNumber) + " " + std::to_string(input));

        if(config.input_motor_one)
        {
            roboteq::Motor* motor = _motor.at(0);
            motor->registerSensor(this);
            ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
        }
        if(config.input_motor_two)
        {
            roboteq::Motor* motor = _motor.at(1);
            motor->registerSensor(this);
            ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
        }
    }
    // Set polarity APOL [pag. 293]
    if(_last_param_config.conversion_polarity != config.conversion_polarity)
    {
        // Update operative mode
        mSerial->setParam("APOL", std::to_string(mNumber) + " " + std::to_string(config.conversion_polarity));
    }
    // Set deadband ADB [pag. 286]
    if(_last_param_config.input_deadband != config.input_deadband)
    {
        // Update operative mode
        mSerial->setParam("ADB", std::to_string(mNumber) + " " + std::to_string(config.input_deadband));
    }
    // Set input AMIN [pag. 290]
    if(_last_param_config.range_input_min != config.range_input_min)
    {
        int range_input_min = config.range_input_min * 1000;
        // Update operative mode
        mSerial->setParam("AMIN", std::to_string(mNumber) + " " + std::to_string(range_input_min));
    }
    // Set input AMAX [pag. 289]
    if(_last_param_config.range_input_max != config.range_input_max)
    {
        int range_input_max = config.range_input_max * 1000;
        // Update operative mode
        mSerial->setParam("AMAX", std::to_string(mNumber) + " " + std::to_string(range_input_max));
    }
    // Set input ATCR [pag. 285]
    if(_last_param_config.range_input_center != config.range_input_center)
    {
        int range_input_center = config.range_input_center * 1000;
        // Update operative mode
        mSerial->setParam("AMAX", std::to_string(mNumber) + " " + std::to_string(range_input_center));
    }
}
<commit_msg>Update gpio_analog.cpp<commit_after>/**
 * Copyright (C) 2017, Raffaello Bonghi <raffaello@rnext.it>
 * All rights reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "configurator/gpio_analog.h"

#define PARAM_ANALOG_STRING "/analog"

GPIOAnalogConfigurator::GPIOAnalogConfigurator(const ros::NodeHandle &nh, roboteq::serial_controller *serial, std::vector<roboteq::Motor *> motor, string name, unsigned int number)
    : nh_(nh)
    , mSerial(serial)
    ,_motor(motor)
{
    // Find path param
    mName = nh_.getNamespace() + name + PARAM_ANALOG_STRING + "/" + std::to_string(number);
    // Roboteq motor number
    mNumber = number;
    // Set false on first run
    setup_param = false;
}

void GPIOAnalogConfigurator::initConfigurator(bool load_from_board)
{
    // Check if is required load paramers
    if(load_from_board)
    {
        // Load parameters from roboteq
        getParamFromRoboteq();
    }
    // Initialize encoder dynamic reconfigure
    mDynRecServer = std::make_shared<ReconfigureServer>(mDynServerMutex, ros::NodeHandle(mName));
    // Load default configuration
    roboteq_control::RoboteqAnalogInputConfig config;
    mDynRecServer->getConfigDefault(config);
    // Update parameters
    mDynServerMutex.lock();
    mDynRecServer->updateConfig(config);
    mDynServerMutex.unlock();
    // Set callback
    mDynRecServer->setCallback(boost::bind(&GPIOAnalogConfigurator::reconfigureCBParam, this, _1, _2));
}

void GPIOAnalogConfigurator::getParamFromRoboteq()
{
    try
    {
        // conversion AMOD [pag. 292]
        string str_conversion = mSerial->getParam("AMOD", std::to_string(mNumber));
        int conversion = boost::lexical_cast<int>(str_conversion);
        // Set params
        nh_.setParam(mName + "/conversion", conversion);

        // input AINA [pag. 287]
        string str_pina = mSerial->getParam("AINA", std::to_string(mNumber));
        // Get AINA from roboteq board
        int emod = boost::lexical_cast<unsigned int>(str_pina);
        // 3 modes:
        // 0 - Unsed
        // 1 - Command
        // 2 - Feedback
        int command = (emod & 0b11);
        int motors = (emod - command) >> 4;
        int tmp1 = ((motors & 0b1) > 0);
        int tmp2 = ((motors & 0b10) > 0);
        if(tmp1)
        {
            for (vector<roboteq::Motor*>::iterator it = _motor.begin() ; it != _motor.end(); ++it)
            {
                roboteq::Motor* motor = ((roboteq::Motor*)(*it));
                if(motor->getNumber() == 1)
                {
                    motor->registerSensor(this);
                    ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
                    break;
                }
            }
        }
        if(tmp2)
        {
            for (vector<roboteq::Motor*>::iterator it = _motor.begin() ; it != _motor.end(); ++it)
            {
                roboteq::Motor* motor = ((roboteq::Motor*)(*it));
                if(motor->getNumber() == 2)
                {
                    motor->registerSensor(this);
                    ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
                    break;
                }
            }
        }

        // Set parameter
        nh_.setParam(mName + "/input_use", command);
        nh_.setParam(mName + "/input_motor_one", tmp1);
        nh_.setParam(mName + "/input_motor_two", tmp2);

        // polarity APOL [pag. 293]
        string str_polarity = mSerial->getParam("APOL", std::to_string(mNumber));
        int polarity = boost::lexical_cast<int>(str_polarity);
        // Set params
        nh_.setParam(mName + "/conversion_polarity", polarity);

        // Input deadband ADB [pag. 286]
        string str_deadband = mSerial->getParam("ADB", std::to_string(mNumber));
        int deadband = boost::lexical_cast<int>(str_deadband);
        // Set params
        nh_.setParam(mName + "/input_deadband", deadband);

        // Input AMIN [pag. 290]
        string str_min = mSerial->getParam("AMIN", std::to_string(mNumber));
        double min = boost::lexical_cast<double>(str_min) / 1000;
        // Set params
        nh_.setParam(mName + "/range_input_min", min);

        // Input AMAX [pag. 289]
        string str_max = mSerial->getParam("AMAX", std::to_string(mNumber));
        double max = boost::lexical_cast<double>(str_max) / 1000;
        // Set params
        nh_.setParam(mName + "/range_input_max", max);

        // Input ATCR [pag. 285]
        string str_ctr = mSerial->getParam("ACTR", std::to_string(mNumber));
        double ctr = boost::lexical_cast<double>(str_ctr) / 1000;
        // Set params
        nh_.setParam(mName + "/range_input_center", ctr);

    } catch (std::bad_cast& e)
    {
        ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what());
    }
}

void GPIOAnalogConfigurator::reconfigureCBParam(roboteq_control::RoboteqAnalogInputConfig &config, uint32_t level)
{
    //The first time we're called, we just want to make sure we have the
    //original configuration
    if(!setup_param)
    {
      _last_param_config = config;
      default_param_config = _last_param_config;
      setup_param = true;
      return;
    }

    if(config.restore_defaults)
    {
        //if someone sets restore defaults on the parameter server, prevent looping
        config.restore_defaults = false;
        // Overload config with default
        config = default_param_config;
    }

    if(config.load_roboteq)
    {
        //if someone sets again the request on the parameter server, prevent looping
        config.load_roboteq = false;
        // Launch param load
        getParamFromRoboteq();
        // Skip other read
        return;
    }

    // Set conversion AMOD [pag. 292]
    if(_last_param_config.conversion != config.conversion)
    {
        // Update operative mode
        mSerial->setParam("AMOD", std::to_string(mNumber) + " " + std::to_string(config.conversion));
    }
    // Set input AINA [pag. 287]
    if((_last_param_config.input_use != config.input_use) ||
            (_last_param_config.input_motor_one != config.input_motor_one) ||
            (_last_param_config.input_motor_two != config.input_motor_two))
    {
        int input = config.input_use + 16*config.input_motor_one + 32*config.input_motor_two;
        mSerial->setParam("AINA", std::to_string(mNumber) + " " + std::to_string(input));

        if(config.input_motor_one)
        {
            roboteq::Motor* motor = _motor.at(0);
            motor->registerSensor(this);
            ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
        }
        if(config.input_motor_two)
        {
            roboteq::Motor* motor = _motor.at(1);
            motor->registerSensor(this);
            ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName());
        }
    }
    // Set polarity APOL [pag. 293]
    if(_last_param_config.conversion_polarity != config.conversion_polarity)
    {
        // Update operative mode
        mSerial->setParam("APOL", std::to_string(mNumber) + " " + std::to_string(config.conversion_polarity));
    }
    // Set deadband ADB [pag. 286]
    if(_last_param_config.input_deadband != config.input_deadband)
    {
        // Update operative mode
        mSerial->setParam("ADB", std::to_string(mNumber) + " " + std::to_string(config.input_deadband));
    }
    // Set input AMIN [pag. 290]
    if(_last_param_config.range_input_min != config.range_input_min)
    {
        int range_input_min = config.range_input_min * 1000;
        // Update operative mode
        mSerial->setParam("AMIN", std::to_string(mNumber) + " " + std::to_string(range_input_min));
    }
    // Set input AMAX [pag. 289]
    if(_last_param_config.range_input_max != config.range_input_max)
    {
        int range_input_max = config.range_input_max * 1000;
        // Update operative mode
        mSerial->setParam("AMAX", std::to_string(mNumber) + " " + std::to_string(range_input_max));
    }
    // Set input ATCR [pag. 285]
    if(_last_param_config.range_input_center != config.range_input_center)
    {
        int range_input_center = config.range_input_center * 1000;
        // Update operative mode
        mSerial->setParam("ATCR", std::to_string(mNumber) + " " + std::to_string(range_input_center));
    }
}
<|endoftext|>


<commit_before>/*
 * MomentumConn.cpp
 *
 *  Created on: Feburary 27, 2014
 *      Author: slundquist
 */

#include "MomentumConn.hpp"
#include <cstring>

namespace PV {

MomentumConn::MomentumConn() { initialize_base(); }

MomentumConn::MomentumConn(
      const char *name,
      HyPerCol *hc,
      InitWeights *weightInitializer,
      NormalizeBase *weightNormalizer)
      : HyPerConn() {
   initialize_base();
   initialize(name, hc, weightInitializer, weightNormalizer);
}

MomentumConn::~MomentumConn() {
   if (momentumMethod) {
      free(momentumMethod);
   }
   if (prev_dwDataStart) {
      free(prev_dwDataStart[0]);
      free(prev_dwDataStart);
   }
}

int MomentumConn::initialize_base() {
   prev_dwDataStart = NULL;
   momentumTau      = .25;
   momentumMethod   = NULL;
   momentumDecay    = 0;
   timeBatchPeriod  = 1;
   timeBatchIdx     = -1;
   return PV_SUCCESS;
}

int MomentumConn::allocateDataStructures() {
   int status = HyPerConn::allocateDataStructures();
   if (status == PV_POSTPONE) {
      return status;
   }
   if (!plasticityFlag)
      return status;
   int sx       = nfp;
   int sy       = sx * nxp;
   int sp       = sy * nyp;
   int nPatches = getNumDataPatches();

   const int numAxons = numberOfAxonalArborLists();

   // Allocate dw buffer for previous dw
   prev_dwDataStart = (pvwdata_t **)pvCalloc(numAxons, sizeof(pvwdata_t *));
   prev_dwDataStart[0] =
         (pvwdata_t *)pvCalloc(numAxons * nxp * nyp * nfp * nPatches, sizeof(pvwdata_t));
   for (int arborId = 0; arborId < numAxons; arborId++) {
      prev_dwDataStart[arborId] = (prev_dwDataStart[0] + sp * nPatches * arborId);
      pvAssert(prev_dwDataStart[arborId] != NULL);
   } // loop over arbors

   return PV_SUCCESS;
}

int MomentumConn::ioParamsFillGroup(enum ParamsIOFlag ioFlag) {
   int status = HyPerConn::ioParamsFillGroup(ioFlag);
   ioParam_momentumMethod(ioFlag);
   ioParam_momentumTau(ioFlag);
   ioParam_momentumDecay(ioFlag);
   ioParam_batchPeriod(ioFlag);
   return status;
}

void MomentumConn::ioParam_momentumTau(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      float defaultVal = 0;
      if (strcmp(momentumMethod, "simple") == 0) {
         defaultVal = .25;
      }
      else if (strcmp(momentumMethod, "viscosity") == 0) {
         defaultVal = 100;
      }
      else if (strcmp(momentumMethod, "alex") == 0) {
         defaultVal = .9;
      }

      parent->parameters()->ioParamValue(ioFlag, name, "momentumTau", &momentumTau, defaultVal);
   }
}

/**
 * @brief momentumMethod: The momentum method to use
 * @details Assuming a = dwMax * pre * post
 * simple: deltaW(t) = a + momentumTau * deltaW(t-1)
 * viscosity: deltaW(t) = (deltaW(t-1) * exp(-1/momentumTau)) + a
 * alex: deltaW(t) = momentumTau * delta(t-1) - momentumDecay * dwMax * w(t) - a
 */
void MomentumConn::ioParam_momentumMethod(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      parent->parameters()->ioParamStringRequired(ioFlag, name, "momentumMethod", &momentumMethod);
      if (strcmp(momentumMethod, "simple") != 0 && strcmp(momentumMethod, "viscosity") != 0
          && strcmp(momentumMethod, "alex")) {
         pvError() << "MomentumConn " << name << ": momentumMethod of " << momentumMethod
                   << " is not known, options are \"simple\", \"viscosity\", and \"alex\"\n";
      }
   }
}

void MomentumConn::ioParam_momentumDecay(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      parent->parameters()->ioParamValue(
            ioFlag, name, "momentumDecay", &momentumDecay, momentumDecay);
      if (momentumDecay < 0 || momentumDecay > 1) {
         pvError() << "MomentumConn " << name
                   << ": momentumDecay must be between 0 and 1 inclusive\n";
      }
   }
}

void MomentumConn::ioParam_batchPeriod(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      parent->parameters()->ioParamValue(
            ioFlag, name, "batchPeriod", &timeBatchPeriod, timeBatchPeriod);
   }
}

int MomentumConn::calc_dW() {
   pvAssert(plasticityFlag);
   int status;
   timeBatchIdx = (timeBatchIdx + 1) % timeBatchPeriod;

   // Clear at time 0, update at time timeBatchPeriod - 1
   bool need_update_w = false;
   bool need_clear_dw = false;
   if (timeBatchIdx == 0) {
      need_clear_dw = true;
   }

   // If updating next timestep, update weights here
   if ((timeBatchIdx + 1) % timeBatchPeriod == 0) {
      need_update_w = true;
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Clear every batch period
      if (need_clear_dw) {
         status = initialize_dW(arborId);
         if (status == PV_BREAK) {
            break;
         }
         pvAssert(status == PV_SUCCESS);
      }
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Sum up parts every timestep
      status = update_dW(arborId);
      if (status == PV_BREAK) {
         break;
      }
      pvAssert(status == PV_SUCCESS);
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Reduce only when we need to update
      if (need_update_w) {
         status = reduce_dW(arborId);
         if (status == PV_BREAK) {
            break;
         }
         pvAssert(status == PV_SUCCESS);
      }
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Normalize only when reduced
      if (need_update_w) {
         status = normalize_dW(arborId);
         if (status == PV_BREAK) {
            break;
         }
         pvAssert(status == PV_SUCCESS);
      }
   }
   return PV_SUCCESS;
}

int MomentumConn::updateWeights(int arborId) {
   if (timeBatchIdx != timeBatchPeriod - 1) {
      return PV_SUCCESS;
   }
   // Add momentum right before updateWeights
   for (int kArbor = 0; kArbor < this->numberOfAxonalArborLists(); kArbor++) {
      applyMomentum(arborId);
   }

   // Saved to prevweights
   pvAssert(prev_dwDataStart);
   std::memcpy(
         *prev_dwDataStart,
         *get_dwDataStart(),
         sizeof(pvwdata_t) * numberOfAxonalArborLists() * nxp * nyp * nfp * getNumDataPatches());

   // add dw to w
   for (int kArbor = 0; kArbor < this->numberOfAxonalArborLists(); kArbor++) {
      pvwdata_t *w_data_start = get_wDataStart(kArbor);
      for (long int k = 0; k < patchStartIndex(getNumDataPatches()); k++) {
         w_data_start[k] += get_dwDataStart(kArbor)[k];
      }
   }
   return PV_BREAK;
}

int MomentumConn::applyMomentum(int arbor_ID) {
   int nExt              = preSynapticLayer()->getNumExtended();
   const PVLayerLoc *loc = preSynapticLayer()->getLayerLoc();
   int numKernels        = getNumDataPatches();
// Shared weights done in parallel, parallel in numkernels
#ifdef PV_USE_OPENMP_THREADS
#pragma omp parallel for
#endif
   for (int kernelIdx = 0; kernelIdx < numKernels; kernelIdx++) {
      pvwdata_t *dwdata_start  = get_dwDataHead(arbor_ID, kernelIdx);
      pvwdata_t *prev_dw_start = get_prev_dwDataHead(arbor_ID, kernelIdx);
      pvwdata_t *wdata_start   = get_wDataHead(arbor_ID, kernelIdx);
      if (!strcmp(momentumMethod, "simple")) {
         for (int k = 0; k < nxp * nyp * nfp; k++) {
            dwdata_start[k] += momentumTau * prev_dw_start[k] - momentumDecay * wdata_start[k];
         }
      }
      else if (!strcmp(momentumMethod, "viscosity")) {
         for (int k = 0; k < nxp * nyp * nfp; k++) {
            dwdata_start[k] = (prev_dw_start[k] * expf(-1.0f / momentumTau)) + dwdata_start[k]
                              - momentumDecay * wdata_start[k];
         }
      }
      else if (!strcmp(momentumMethod, "alex")) {
         for (int k = 0; k < nxp * nyp * nfp; k++) {
            dwdata_start[k] = momentumTau * prev_dw_start[k]
                              - momentumDecay * getDWMax() * wdata_start[k] + dwdata_start[k];
         }
      }
   }
   return PV_SUCCESS;
}

// TODO checkpointing not working with batching, must write checkpoint exactly at period
int MomentumConn::registerData(Checkpointer *checkpointer, std::string const &objName) {
   int status = HyPerConn::registerData(checkpointer, objName);
   if (!plasticityFlag) {
      return status;
   }
   checkpointWeightPvp(checkpointer, "prev_dW", prev_dwDataStart);
   return status;
}

} // end namespace PV
<commit_msg>Hotfix: nPatches needs to be cast to size_t<commit_after>/*
 * MomentumConn.cpp
 *
 *  Created on: Feburary 27, 2014
 *      Author: slundquist
 */

#include "MomentumConn.hpp"
#include <cstring>

namespace PV {

MomentumConn::MomentumConn() { initialize_base(); }

MomentumConn::MomentumConn(
      const char *name,
      HyPerCol *hc,
      InitWeights *weightInitializer,
      NormalizeBase *weightNormalizer)
      : HyPerConn() {
   initialize_base();
   initialize(name, hc, weightInitializer, weightNormalizer);
}

MomentumConn::~MomentumConn() {
   if (momentumMethod) {
      free(momentumMethod);
   }
   if (prev_dwDataStart) {
      free(prev_dwDataStart[0]);
      free(prev_dwDataStart);
   }
}

int MomentumConn::initialize_base() {
   prev_dwDataStart = NULL;
   momentumTau      = .25;
   momentumMethod   = NULL;
   momentumDecay    = 0;
   timeBatchPeriod  = 1;
   timeBatchIdx     = -1;
   return PV_SUCCESS;
}

int MomentumConn::allocateDataStructures() {
   int status = HyPerConn::allocateDataStructures();
   if (status == PV_POSTPONE) {
      return status;
   }
   if (!plasticityFlag)
      return status;
   int sx       = nfp;
   int sy       = sx * nxp;
   int sp       = sy * nyp;
   int nPatches = getNumDataPatches();

   const int numAxons = numberOfAxonalArborLists();

   // Allocate dw buffer for previous dw
   prev_dwDataStart       = (pvwdata_t **)pvCalloc(numAxons, sizeof(pvwdata_t *));
   std::size_t numWeights = (std::size_t)(numAxons * nxp * nyp * nfp) * (std::size_t)nPatches;
   prev_dwDataStart[0]    = (pvwdata_t *)pvCalloc(numWeights, sizeof(pvwdata_t));
   for (int arborId = 0; arborId < numAxons; arborId++) {
      prev_dwDataStart[arborId] = (prev_dwDataStart[0] + sp * nPatches * arborId);
      pvAssert(prev_dwDataStart[arborId] != NULL);
   } // loop over arbors

   return PV_SUCCESS;
}

int MomentumConn::ioParamsFillGroup(enum ParamsIOFlag ioFlag) {
   int status = HyPerConn::ioParamsFillGroup(ioFlag);
   ioParam_momentumMethod(ioFlag);
   ioParam_momentumTau(ioFlag);
   ioParam_momentumDecay(ioFlag);
   ioParam_batchPeriod(ioFlag);
   return status;
}

void MomentumConn::ioParam_momentumTau(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      float defaultVal = 0;
      if (strcmp(momentumMethod, "simple") == 0) {
         defaultVal = .25;
      }
      else if (strcmp(momentumMethod, "viscosity") == 0) {
         defaultVal = 100;
      }
      else if (strcmp(momentumMethod, "alex") == 0) {
         defaultVal = .9;
      }

      parent->parameters()->ioParamValue(ioFlag, name, "momentumTau", &momentumTau, defaultVal);
   }
}

/**
 * @brief momentumMethod: The momentum method to use
 * @details Assuming a = dwMax * pre * post
 * simple: deltaW(t) = a + momentumTau * deltaW(t-1)
 * viscosity: deltaW(t) = (deltaW(t-1) * exp(-1/momentumTau)) + a
 * alex: deltaW(t) = momentumTau * delta(t-1) - momentumDecay * dwMax * w(t) - a
 */
void MomentumConn::ioParam_momentumMethod(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      parent->parameters()->ioParamStringRequired(ioFlag, name, "momentumMethod", &momentumMethod);
      if (strcmp(momentumMethod, "simple") != 0 && strcmp(momentumMethod, "viscosity") != 0
          && strcmp(momentumMethod, "alex")) {
         pvError() << "MomentumConn " << name << ": momentumMethod of " << momentumMethod
                   << " is not known, options are \"simple\", \"viscosity\", and \"alex\"\n";
      }
   }
}

void MomentumConn::ioParam_momentumDecay(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      parent->parameters()->ioParamValue(
            ioFlag, name, "momentumDecay", &momentumDecay, momentumDecay);
      if (momentumDecay < 0 || momentumDecay > 1) {
         pvError() << "MomentumConn " << name
                   << ": momentumDecay must be between 0 and 1 inclusive\n";
      }
   }
}

void MomentumConn::ioParam_batchPeriod(enum ParamsIOFlag ioFlag) {
   pvAssert(!parent->parameters()->presentAndNotBeenRead(name, "plasticityFlag"));
   if (plasticityFlag) {
      parent->parameters()->ioParamValue(
            ioFlag, name, "batchPeriod", &timeBatchPeriod, timeBatchPeriod);
   }
}

int MomentumConn::calc_dW() {
   pvAssert(plasticityFlag);
   int status;
   timeBatchIdx = (timeBatchIdx + 1) % timeBatchPeriod;

   // Clear at time 0, update at time timeBatchPeriod - 1
   bool need_update_w = false;
   bool need_clear_dw = false;
   if (timeBatchIdx == 0) {
      need_clear_dw = true;
   }

   // If updating next timestep, update weights here
   if ((timeBatchIdx + 1) % timeBatchPeriod == 0) {
      need_update_w = true;
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Clear every batch period
      if (need_clear_dw) {
         status = initialize_dW(arborId);
         if (status == PV_BREAK) {
            break;
         }
         pvAssert(status == PV_SUCCESS);
      }
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Sum up parts every timestep
      status = update_dW(arborId);
      if (status == PV_BREAK) {
         break;
      }
      pvAssert(status == PV_SUCCESS);
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Reduce only when we need to update
      if (need_update_w) {
         status = reduce_dW(arborId);
         if (status == PV_BREAK) {
            break;
         }
         pvAssert(status == PV_SUCCESS);
      }
   }

   for (int arborId = 0; arborId < numberOfAxonalArborLists(); arborId++) {
      // Normalize only when reduced
      if (need_update_w) {
         status = normalize_dW(arborId);
         if (status == PV_BREAK) {
            break;
         }
         pvAssert(status == PV_SUCCESS);
      }
   }
   return PV_SUCCESS;
}

int MomentumConn::updateWeights(int arborId) {
   if (timeBatchIdx != timeBatchPeriod - 1) {
      return PV_SUCCESS;
   }
   // Add momentum right before updateWeights
   for (int kArbor = 0; kArbor < this->numberOfAxonalArborLists(); kArbor++) {
      applyMomentum(arborId);
   }

   // Saved to prevweights
   pvAssert(prev_dwDataStart);
   std::memcpy(
         *prev_dwDataStart,
         *get_dwDataStart(),
         sizeof(pvwdata_t) * numberOfAxonalArborLists() * nxp * nyp * nfp * getNumDataPatches());

   // add dw to w
   for (int kArbor = 0; kArbor < this->numberOfAxonalArborLists(); kArbor++) {
      pvwdata_t *w_data_start = get_wDataStart(kArbor);
      for (long int k = 0; k < patchStartIndex(getNumDataPatches()); k++) {
         w_data_start[k] += get_dwDataStart(kArbor)[k];
      }
   }
   return PV_BREAK;
}

int MomentumConn::applyMomentum(int arbor_ID) {
   int nExt              = preSynapticLayer()->getNumExtended();
   const PVLayerLoc *loc = preSynapticLayer()->getLayerLoc();
   int numKernels        = getNumDataPatches();
// Shared weights done in parallel, parallel in numkernels
#ifdef PV_USE_OPENMP_THREADS
#pragma omp parallel for
#endif
   for (int kernelIdx = 0; kernelIdx < numKernels; kernelIdx++) {
      pvwdata_t *dwdata_start  = get_dwDataHead(arbor_ID, kernelIdx);
      pvwdata_t *prev_dw_start = get_prev_dwDataHead(arbor_ID, kernelIdx);
      pvwdata_t *wdata_start   = get_wDataHead(arbor_ID, kernelIdx);
      if (!strcmp(momentumMethod, "simple")) {
         for (int k = 0; k < nxp * nyp * nfp; k++) {
            dwdata_start[k] += momentumTau * prev_dw_start[k] - momentumDecay * wdata_start[k];
         }
      }
      else if (!strcmp(momentumMethod, "viscosity")) {
         for (int k = 0; k < nxp * nyp * nfp; k++) {
            dwdata_start[k] = (prev_dw_start[k] * expf(-1.0f / momentumTau)) + dwdata_start[k]
                              - momentumDecay * wdata_start[k];
         }
      }
      else if (!strcmp(momentumMethod, "alex")) {
         for (int k = 0; k < nxp * nyp * nfp; k++) {
            dwdata_start[k] = momentumTau * prev_dw_start[k]
                              - momentumDecay * getDWMax() * wdata_start[k] + dwdata_start[k];
         }
      }
   }
   return PV_SUCCESS;
}

// TODO checkpointing not working with batching, must write checkpoint exactly at period
int MomentumConn::registerData(Checkpointer *checkpointer, std::string const &objName) {
   int status = HyPerConn::registerData(checkpointer, objName);
   if (!plasticityFlag) {
      return status;
   }
   checkpointWeightPvp(checkpointer, "prev_dW", prev_dwDataStart);
   return status;
}

} // end namespace PV
<|endoftext|>


<commit_before>#include "MultiLevelProblem.hpp"
#include "NumericVector.hpp"
#include "Fluid.hpp"
#include "Solid.hpp"
#include "Parameter.hpp"
#include "FemusInit.hpp"
#include "SparseMatrix.hpp"
#include "FElemTypeEnum.hpp"
#include "Files.hpp"
#include "MonolithicFSINonLinearImplicitSystem.hpp"
#include "../../include/FSISteadyStateAssembly.hpp"
#include "VTKWriter.hpp"

double scale = 1000.;

using namespace std;
using namespace femus;

bool SetBoundaryCondition(const std::vector < double >& x, const char name[],
                          double &value, const int facename, const double time);
bool SetBoundaryConditionTurek(const std::vector < double >& x, const char name[],
			       double &value, const int facename, const double time);
//------------------------------------------------------------------------------------------------------------------

int main(int argc, char **args) {

  // ******* Init Petsc-MPI communicator *******
  FemusInit mpinit(argc, args, MPI_COMM_WORLD);

  bool dimension2D = false;

  //Files files;
  //files.CheckIODirectories();
  //files.RedirectCout();

  // ******* Extract the problem dimension and simulation identifier based on the inline input *******

  // ******* Extract the mesh.neu file name based on the simulation identifier *******
//   std::string infile = "./input/aneurysm_Sara_5.neu";
  //std::string infile = "./input/aneurisma_aorta.neu";
  std::string infile = "./input/Turek_3D_D.neu";

  // ******* Set physics parameters *******
  double Lref, Uref, rhof, muf, rhos, ni, E;

  Lref = 1.;
  Uref = 1.;

  rhof = 1035.;
  muf = 3.38*1.0e-4*rhof;
  rhos = 1120;
  ni = 0.5;
  E = 120000*1.e4;

  Parameter par(Lref, Uref);

  // Generate Solid Object
  Solid solid;
  solid = Solid(par, E, ni, rhos, "Mooney-Rivlin");

  cout << "Solid properties: " << endl;
  cout << solid << endl;

  // Generate Fluid Object
  Fluid fluid(par, muf, rhof, "Newtonian");
  cout << "Fluid properties: " << endl;
  cout << fluid << endl;

  // ******* Init multilevel mesh from mesh.neu file *******
  unsigned short numberOfUniformRefinedMeshes, numberOfAMRLevels;

  numberOfUniformRefinedMeshes = 2;
  numberOfAMRLevels = 0;

  std::cout << 0 << std::endl;

  MultiLevelMesh ml_msh(numberOfUniformRefinedMeshes + numberOfAMRLevels, numberOfUniformRefinedMeshes,
                        infile.c_str(), "fifth", Lref, NULL);

  //ml_msh.EraseCoarseLevels(numberOfUniformRefinedMeshes - 1);

  ml_msh.PrintInfo();

  // mark Solid nodes
  ml_msh.MarkStructureNode();

  // ******* Init multilevel solution ******
  MultiLevelSolution ml_sol(&ml_msh);

  // ******* Add solution variables to multilevel solution and pair them *******
  ml_sol.AddSolution("DX", LAGRANGE, SECOND, 1);
  ml_sol.AddSolution("DY", LAGRANGE, SECOND, 1);
  if(!dimension2D) ml_sol.AddSolution("DZ", LAGRANGE, SECOND, 1);

  ml_sol.AddSolution("U", LAGRANGE, SECOND, 1);
  ml_sol.AddSolution("V", LAGRANGE, SECOND, 1);
  if(!dimension2D) ml_sol.AddSolution("W", LAGRANGE, SECOND, 1);

  // Pair each velocity variable with the corresponding displacement variable
  ml_sol.PairSolution("U", "DX"); // Add this line
  ml_sol.PairSolution("V", "DY"); // Add this line
  if(!dimension2D) ml_sol.PairSolution("W", "DZ"); // Add this line

  // Since the Pressure is a Lagrange multiplier it is used as an implicit variable
  ml_sol.AddSolution("P", DISCONTINOUS_POLYNOMIAL, FIRST, 1);
  ml_sol.AssociatePropertyToSolution("P", "Pressure", false); // Add this line

  // ******* Initialize solution *******
  ml_sol.Initialize("All");
  //ml_sol.AttachSetBoundaryConditionFunction(SetBoundaryCondition);
  ml_sol.AttachSetBoundaryConditionFunction(SetBoundaryConditionTurek);

  // ******* Set boundary conditions *******
  ml_sol.GenerateBdc("DX", "Steady");
  ml_sol.GenerateBdc("DY", "Steady");
  if(!dimension2D) ml_sol.GenerateBdc("DZ", "Steady");
  ml_sol.GenerateBdc("U", "Steady");
  ml_sol.GenerateBdc("V", "Steady");
  if(!dimension2D) ml_sol.GenerateBdc("W", "Steady");
  ml_sol.GenerateBdc("P", "Steady");

  // ******* Define the FSI Multilevel Problem *******

  MultiLevelProblem ml_prob(&ml_sol);
  // Add fluid object
  ml_prob.parameters.set<Fluid>("Fluid") = fluid;
  // Add Solid Object
  ml_prob.parameters.set<Solid>("Solid") = solid;

  // ******* Add FSI system to the MultiLevel problem *******
  MonolithicFSINonLinearImplicitSystem & system = ml_prob.add_system<MonolithicFSINonLinearImplicitSystem> ("Fluid-Structure-Interaction");
  system.AddSolutionToSystemPDE("DX");
  system.AddSolutionToSystemPDE("DY");
  if(!dimension2D) system.AddSolutionToSystemPDE("DZ");
  system.AddSolutionToSystemPDE("U");
  system.AddSolutionToSystemPDE("V");
  if(!dimension2D) system.AddSolutionToSystemPDE("W");
  system.AddSolutionToSystemPDE("P");

  // ******* System Fluid-Structure-Interaction Assembly *******
  system.SetAssembleFunction(FSISteadyStateAssembly);

  // ******* set MG-Solver *******
  system.SetMgType(F_CYCLE);

  system.SetNonLinearConvergenceTolerance(1.e-9);
  system.SetResidualUpdateConvergenceTolerance(1.e-15);
  system.SetMaxNumberOfNonLinearIterations(4);
  system.SetMaxNumberOfResidualUpdatesForNonlinearIteration(1);

  system.SetNumberPreSmoothingStep(0);
  system.SetNumberPostSmoothingStep(2);

  // ******* Set Preconditioner *******

  system.SetMgSmoother(ASM_SMOOTHER);

  system.init();

  // ******* Set Smoother *******
  system.SetSolverFineGrids(RICHARDSON);
  //system.SetSolverFineGrids(GMRES);

  system.SetPreconditionerFineGrids(ILU_PRECOND);

  system.SetTolerances(1.e-12, 1.e-20, 1.e+50, 20, 10);

  // ******* Add variables to be solved *******
  system.ClearVariablesToBeSolved();
  system.AddVariableToBeSolved("All");

  // ******* Set the last (1) variables in system (i.e. P) to be a schur variable *******
  system.SetNumberOfSchurVariables(1);

  // ******* Set block size for the ASM smoothers *******
  system.SetElementBlockNumber(2);

  // ******* For Gmres Preconditioner only *******
  //system.SetDirichletBCsHandling(ELIMINATION);

  // ******* Print solution *******
  ml_sol.SetWriter(VTK);

  std::vector<std::string> mov_vars;
  mov_vars.push_back("DX");
  mov_vars.push_back("DY");
  mov_vars.push_back("DZ");
  ml_sol.GetWriter()->SetMovingMesh(mov_vars);

  std::vector<std::string> print_vars;
  print_vars.push_back("All");

  ml_sol.GetWriter()->SetDebugOutput(true);
  ml_sol.GetWriter()->Write(DEFAULT_OUTPUTDIR,"biquadratic",print_vars,0);

  // ******* Solve *******
  std::cout << std::endl;
  std::cout << " *********** Fluid-Structure-Interaction ************  " << std::endl;
  //system.MGsolve();

  ml_sol.GetWriter()->Write(DEFAULT_OUTPUTDIR,"biquadratic",print_vars,1);

  // ******* Clear all systems *******
  ml_prob.clear();
  return 0;
}

//---------------------------------------------------------------------------------------------------------------------

bool SetBoundaryCondition(const std::vector < double >& x, const char name[], double &value, const int facename, const double time) {
  bool test = 1; //dirichlet
  value = 0.;

  if(!strcmp(name, "U") || !strcmp(name, "V") || !strcmp(name, "W")) {

    if(1 == facename) {
      test = 0;
      value = 10.;
    }
    else if( 2 == facename ) {
      test = 0;
      value = 10.;

    }
    else if( 3 == facename) {
      test = 0;
      value = 20.;
    }
  }

  else if(!strcmp(name, "P")) {
    test = 0;
    value = 0.;
  }

  else if(!strcmp(name, "DX") || !strcmp(name, "DY") || !strcmp(name, "DZ")) {
    if(7 == facename) {
      test = 0;
      value = 1.;
    }
  }

  return test;

}

bool SetBoundaryConditionTurek(const std::vector < double >& x, const char name[], double &value, const int facename, const double time) {
  bool test = 1; //dirichlet
  value = 0.;

if( !strcmp(name, "U") ){

    if(1 == facename) {
      value = -0.05 * (x[1]*1000 - 6) * ( x[1]*1000 - 8); //inflow
    }
    else if( 2 == facename ){
      test = 0;
      value = 0.;
    }
  }
  else if( !strcmp(name, "V") || !strcmp(name, "W") ){
    if( 2 == facename ){
      test = 0;
      value = 0.;
    }
  }
  else if(!strcmp(name, "P")) {
    test = 0;
    value = 0.;
  }
  else if(!strcmp(name, "DX") ) {
    if(2 == facename || 4 == facename || 5 == facename || 6 == facename) {
      test = 0;
      value = 0;
    }
  }
  else if(!strcmp(name, "DY") ) {
    if(1 == facename || 3 == facename || 5 == facename || 6 == facename) {
      test = 0;
      value = 0;
    }
  }
  else if(!strcmp(name, "DZ") ) {
    if(5 == facename || 6 == facename) {
      test = 0;
      value = 0;
    }
  }

  return test;

}
<commit_msg>added moving 3D aneurysm<commit_after>#include "MultiLevelProblem.hpp"
#include "NumericVector.hpp"
#include "Fluid.hpp"
#include "Solid.hpp"
#include "Parameter.hpp"
#include "FemusInit.hpp"
#include "SparseMatrix.hpp"
#include "FElemTypeEnum.hpp"
#include "Files.hpp"
#include "MonolithicFSINonLinearImplicitSystem.hpp"
#include "../../include/FSISteadyStateAssembly.hpp"
#include "VTKWriter.hpp"

double scale = 1000.;

using namespace std;
using namespace femus;

bool SetBoundaryCondition(const std::vector < double >& x, const char name[],
                          double &value, const int facename, const double time);
bool SetBoundaryConditionTurek(const std::vector < double >& x, const char name[],
			       double &value, const int facename, const double time);
//------------------------------------------------------------------------------------------------------------------

int main(int argc, char **args) {

  // ******* Init Petsc-MPI communicator *******
  FemusInit mpinit(argc, args, MPI_COMM_WORLD);

  bool dimension2D = false;

  //Files files;
  //files.CheckIODirectories();
  //files.RedirectCout();

  // ******* Extract the problem dimension and simulation identifier based on the inline input *******

  // ******* Extract the mesh.neu file name based on the simulation identifier *******
//   std::string infile = "./input/aneurysm_Sara_5.neu";
  //std::string infile = "./input/aneurisma_aorta.neu";
  std::string infile = "./input/Turek_3D_D.neu";

  // ******* Set physics parameters *******
  double Lref, Uref, rhof, muf, rhos, ni, E;

  Lref = 1.;
  Uref = 1.;

  rhof = 1035.;
  muf = 3.38*1.0e-4*rhof;
  rhos = 1120;
  ni = 0.5;
  E = 120000*1.e1;

  Parameter par(Lref, Uref);

  // Generate Solid Object
  Solid solid;
  solid = Solid(par, E, ni, rhos, "Mooney-Rivlin");

  cout << "Solid properties: " << endl;
  cout << solid << endl;

  // Generate Fluid Object
  Fluid fluid(par, muf, rhof, "Newtonian");
  cout << "Fluid properties: " << endl;
  cout << fluid << endl;

  // ******* Init multilevel mesh from mesh.neu file *******
  unsigned short numberOfUniformRefinedMeshes, numberOfAMRLevels;

  numberOfUniformRefinedMeshes = 2;
  numberOfAMRLevels = 0;

  std::cout << 0 << std::endl;

  MultiLevelMesh ml_msh(numberOfUniformRefinedMeshes + numberOfAMRLevels, numberOfUniformRefinedMeshes,
                        infile.c_str(), "fifth", Lref, NULL);

  //ml_msh.EraseCoarseLevels(numberOfUniformRefinedMeshes - 1);

  ml_msh.PrintInfo();

  // mark Solid nodes
  ml_msh.MarkStructureNode();

  // ******* Init multilevel solution ******
  MultiLevelSolution ml_sol(&ml_msh);

  // ******* Add solution variables to multilevel solution and pair them *******
  ml_sol.AddSolution("DX", LAGRANGE, SECOND, 1);
  ml_sol.AddSolution("DY", LAGRANGE, SECOND, 1);
  if(!dimension2D) ml_sol.AddSolution("DZ", LAGRANGE, SECOND, 1);

  ml_sol.AddSolution("U", LAGRANGE, SECOND, 1);
  ml_sol.AddSolution("V", LAGRANGE, SECOND, 1);
  if(!dimension2D) ml_sol.AddSolution("W", LAGRANGE, SECOND, 1);

  // Pair each velocity variable with the corresponding displacement variable
  ml_sol.PairSolution("U", "DX"); // Add this line
  ml_sol.PairSolution("V", "DY"); // Add this line
  if(!dimension2D) ml_sol.PairSolution("W", "DZ"); // Add this line

  // Since the Pressure is a Lagrange multiplier it is used as an implicit variable
  ml_sol.AddSolution("P", DISCONTINOUS_POLYNOMIAL, FIRST, 1);
  ml_sol.AssociatePropertyToSolution("P", "Pressure", false); // Add this line

  // ******* Initialize solution *******
  ml_sol.Initialize("All");
  //ml_sol.AttachSetBoundaryConditionFunction(SetBoundaryCondition);
  ml_sol.AttachSetBoundaryConditionFunction(SetBoundaryConditionTurek);

  // ******* Set boundary conditions *******
  ml_sol.GenerateBdc("DX", "Steady");
  ml_sol.GenerateBdc("DY", "Steady");
  if(!dimension2D) ml_sol.GenerateBdc("DZ", "Steady");
  ml_sol.GenerateBdc("U", "Steady");
  ml_sol.GenerateBdc("V", "Steady");
  if(!dimension2D) ml_sol.GenerateBdc("W", "Steady");
  ml_sol.GenerateBdc("P", "Steady");

  // ******* Define the FSI Multilevel Problem *******

  MultiLevelProblem ml_prob(&ml_sol);
  // Add fluid object
  ml_prob.parameters.set<Fluid>("Fluid") = fluid;
  // Add Solid Object
  ml_prob.parameters.set<Solid>("Solid") = solid;

  // ******* Add FSI system to the MultiLevel problem *******
  MonolithicFSINonLinearImplicitSystem & system = ml_prob.add_system<MonolithicFSINonLinearImplicitSystem> ("Fluid-Structure-Interaction");
  system.AddSolutionToSystemPDE("DX");
  system.AddSolutionToSystemPDE("DY");
  if(!dimension2D) system.AddSolutionToSystemPDE("DZ");
  system.AddSolutionToSystemPDE("U");
  system.AddSolutionToSystemPDE("V");
  if(!dimension2D) system.AddSolutionToSystemPDE("W");
  system.AddSolutionToSystemPDE("P");

  // ******* System Fluid-Structure-Interaction Assembly *******
  system.SetAssembleFunction(FSISteadyStateAssembly);

  // ******* set MG-Solver *******
  system.SetMgType(F_CYCLE);

  system.SetNonLinearConvergenceTolerance(1.e-9);
  system.SetResidualUpdateConvergenceTolerance(1.e-15);
  system.SetMaxNumberOfNonLinearIterations(4);
  system.SetMaxNumberOfResidualUpdatesForNonlinearIteration(1);

  system.SetNumberPreSmoothingStep(0);
  system.SetNumberPostSmoothingStep(2);

  // ******* Set Preconditioner *******

  system.SetMgSmoother(ASM_SMOOTHER);

  system.init();

  // ******* Set Smoother *******
  system.SetSolverFineGrids(RICHARDSON);
  //system.SetSolverFineGrids(GMRES);

  system.SetPreconditionerFineGrids(ILU_PRECOND);

  system.SetTolerances(1.e-12, 1.e-20, 1.e+50, 20, 10);

  // ******* Add variables to be solved *******
  system.ClearVariablesToBeSolved();
  system.AddVariableToBeSolved("All");

  // ******* Set the last (1) variables in system (i.e. P) to be a schur variable *******
  system.SetNumberOfSchurVariables(1);

  // ******* Set block size for the ASM smoothers *******
  system.SetElementBlockNumber(2);

  // ******* For Gmres Preconditioner only *******
  //system.SetDirichletBCsHandling(ELIMINATION);

  // ******* Print solution *******
  ml_sol.SetWriter(VTK);

  std::vector<std::string> mov_vars;
  mov_vars.push_back("DX");
  mov_vars.push_back("DY");
  mov_vars.push_back("DZ");
  ml_sol.GetWriter()->SetMovingMesh(mov_vars);

  std::vector<std::string> print_vars;
  print_vars.push_back("All");

  ml_sol.GetWriter()->SetDebugOutput(true);
  ml_sol.GetWriter()->Write(DEFAULT_OUTPUTDIR,"biquadratic",print_vars,0);

  // ******* Solve *******
  std::cout << std::endl;
  std::cout << " *********** Fluid-Structure-Interaction ************  " << std::endl;
  system.MGsolve();

  ml_sol.GetWriter()->Write(DEFAULT_OUTPUTDIR,"biquadratic",print_vars,1);

  // ******* Clear all systems *******
  ml_prob.clear();
  return 0;
}

//---------------------------------------------------------------------------------------------------------------------

bool SetBoundaryCondition(const std::vector < double >& x, const char name[], double &value, const int facename, const double time) {
  bool test = 1; //dirichlet
  value = 0.;

  if(!strcmp(name, "U") || !strcmp(name, "V") || !strcmp(name, "W")) {

    if(1 == facename) {
      test = 0;
      value = 10.;
    }
    else if( 2 == facename ) {
      test = 0;
      value = 10.;

    }
    else if( 3 == facename) {
      test = 0;
      value = 20.;
    }
  }

  else if(!strcmp(name, "P")) {
    test = 0;
    value = 0.;
  }

  else if(!strcmp(name, "DX") || !strcmp(name, "DY") || !strcmp(name, "DZ")) {
    if(7 == facename) {
      test = 0;
      value = 1.;
    }
  }

  return test;

}

bool SetBoundaryConditionTurek(const std::vector < double >& x, const char name[], double &value, const int facename, const double time) {
  bool test = 1; //dirichlet
  value = 0.;

if( !strcmp(name, "U") ){

    if(1 == facename) {
      double r2 = ((x[1]*1000.)-7.)*((x[1]*1000.)-7.) + (x[2]*1000.)*(x[2]*1000.);
      value = 0.05 * (1.-r2); //inflow
    }
    else if( 2 == facename ){
      test = 0;
      value = 0.;
    }
  }
  else if( !strcmp(name, "V") || !strcmp(name, "W") ){
    if( 2 == facename ){
      test = 0;
      value = 0.;
    }
  }
  else if(!strcmp(name, "P")) {
    test = 0;
    value = 0.;
  }
  else if(!strcmp(name, "DX") ) {
    if(2 == facename || 4 == facename || 5 == facename || 6 == facename) {
      test = 0;
      value = 0;
    }
  }
  else if(!strcmp(name, "DY") ) {
    if(1 == facename || 3 == facename || 5 == facename || 6 == facename) {
      test = 0;
      value = 0;
    }
  }
  else if(!strcmp(name, "DZ") ) {
    if(1 == facename || 3 == facename || 5 == facename || 6 == facename) {
      test = 0;
      value = 0;
    }
  }

  return test;

}
<|endoftext|>