text:
#ifndef FLEMU_PROXY_HPP
#define FLEMU_PROXY_HPP

#include "utility.hpp"

#include <boost/ut.hpp>

#include <cassert>
#include <cstdint>

#include <compare>
#include <concepts>

namespace flemu
{

template<std::unsigned_integral Base>
struct bit_proxy
{
  public:

    using base_type = Base;

  public:

    // [start, stop] both ends are included
    constexpr bit_proxy(base_type& b, const std::size_t start, const std::size_t stop) noexcept
      : base_(std::addressof(b)), start_(std::min(start, stop)), stop_(std::max(start, stop)),
        mask_(mask<base_type>(0, start_-1) | mask<base_type>(stop_+1, sizeof(base_type)*8))
    {
        assert(stop_ < sizeof(base_type)*8);
    }

    constexpr bit_proxy& operator=(base_type i)
    {
        (*base_) &= mask_;
        (*base_) += ((i << start_) & mask<base_type>(stop_, start_));
        return *this;
    }

    constexpr explicit operator base_type() const
    {
        const base_type x = (*base_);
        return (x & mask<base_type>(stop_, start_)) >> start_;
    }

    constexpr auto operator==(const base_type& other) const
    {
        return other == base_type(*this);
    }
    constexpr auto operator<=>(const base_type& other) const
    {
        return other <=> base_type(*this);
    }

    constexpr auto operator==(const bit_proxy& other) const
    {
        return base_type(*this) == base_type(other);
    }
    constexpr auto operator<=>(const bit_proxy& other) const
    {
        return base_type(*this) <=> base_type(other);
    }

    constexpr std::size_t start() const noexcept {return start_;}
    constexpr std::size_t stop()  const noexcept {return stop_;}
    constexpr std::size_t width() const noexcept {return stop_ - start_ + 1;}

  private:
    base_type*  base_;
    std::size_t start_;
    std::size_t stop_;
    base_type   mask_;
};

template<typename charT, typename traits, typename Base>
std::basic_ostream<charT, traits>&
operator<<(std::basic_ostream<charT, traits>& os, const bit_proxy<Base>& bp)
{
    os << Base(bp);
    return os;
}

template<std::unsigned_integral Base>
struct const_bit_proxy
{
  public:

    using base_type = Base;

  public:

    // [start, stop] both ends are included
    constexpr const_bit_proxy(const base_type& b, const std::size_t start, const std::size_t stop) noexcept
      : base_(std::addressof(b)), start_(std::min(start, stop)), stop_(std::max(start, stop)),
        mask_(mask<base_type>(0, start_-1) | mask<base_type>(stop_+1, sizeof(base_type)*8))
    {
        assert(stop_ < sizeof(base_type)*8);
    }

    constexpr explicit operator base_type() const
    {
        const auto x = (*base_);
        return (x & mask<base_type>(stop_, start_)) >> start_;
    }

    constexpr auto operator==(const base_type& other) const
    {
        return other == base_type(*this);
    }
    constexpr auto operator<=>(const base_type& other) const
    {
        return other <=> base_type(*this);
    }

    constexpr auto operator==(const const_bit_proxy& other) const
    {
        return base_type(*this) == base_type(other);
    }
    constexpr auto operator<=>(const const_bit_proxy& other) const
    {
        return base_type(*this) <=> base_type(other);
    }

    constexpr std::size_t start() const noexcept {return start_;}
    constexpr std::size_t stop()  const noexcept {return stop_;}
    constexpr std::size_t width() const noexcept {return stop_ - start_ + 1;}

  private:
    base_type const* base_;
    std::size_t start_;
    std::size_t stop_;
    std::size_t width_;
    base_type   mask_;
};

template<typename charT, typename traits, typename Base>
std::basic_ostream<charT, traits>&
operator<<(std::basic_ostream<charT, traits>& os, const const_bit_proxy<Base>& bp)
{
    os << Base(bp);
    return os;
}

#ifdef FLEMU_ACTIVATE_UNIT_TESTS
inline boost::ut::suite tests_bit_proxy = []
{
    using namespace boost::ut::literals;

    "bit_proxy_substitution"_test = []
    {
        std::uint32_t u32 = 0x00FF'0F0F;
        bit_proxy proxy1(u32, 15, 0);

        boost::ut::expect(proxy1.start() ==  0);
        boost::ut::expect(proxy1.stop()  == 15);
        boost::ut::expect(proxy1.width() == 16);

        const std::uint32_t proxy1_data(proxy1);
        boost::ut::expect(proxy1      == 0x0F0F);
        boost::ut::expect(proxy1_data == 0x0F0F);

        proxy1 = 0xF0F0;
        boost::ut::expect(u32 == 0x00FF'F0F0);

        bit_proxy proxy2(u32, 23, 8);
        boost::ut::expect(proxy2 == 0xFFF0);

        proxy2 = 0x000F;
        boost::ut::expect(u32 == 0x0000'0FF0);
        boost::ut::expect(proxy2 == 0x000F);

        bit_proxy proxy3(u32, 31, 16);
        boost::ut::expect(proxy3 == 0x0000);

        proxy3 = 0xDEAD;
        boost::ut::expect(u32 == 0xDEAD'0FF0);
        boost::ut::expect(proxy3 == 0xDEAD);

        proxy1 = 0xBEEF'BEEF;
        boost::ut::expect(u32 == 0xDEAD'BEEF);

        bit_proxy proxy4(u32, 31, 31);
        boost::ut::expect(proxy4 == 1);
    };

    "bit_proxy_comparison"_test = []
    {
        std::uint32_t u32 = 0x00FF'0F0F;
        bit_proxy proxy1(u32, 15,  0); // 0F0F
        bit_proxy proxy2(u32, 23,  8); // FF0F
        bit_proxy proxy3(u32, 31, 16); // 00FF

        boost::ut::expect(proxy1 < proxy2);
        boost::ut::expect(proxy1 > proxy3);
        boost::ut::expect(proxy2 > proxy3);
    };
};
#endif

} // flemu
#endif// FLEMU_PROXY_HPP

ext: hpp

max_stars_repo_name: ToruNiina/flemu

max_issues_repo_name: ToruNiina/flemu

max_forks_repo_name: ToruNiina/flemu


text:
/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2015 by Horatiu George Todoran <todorangrg@gmail.com>   *
 *                                                                         *
 *   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 <algorithm>
#include <assert.h>

#include <tuw_gazebo_plugins/gazebo_ros_bridge_model.h>

#include <sdf/sdf.hh>
#include <boost/graph/graph_concepts.hpp>

#include <ros/ros.h>

using namespace std;
// using namespace tuw;

namespace gazebo {

GazeboRosBridgeModelPlugin::GazeboRosBridgeModelPlugin() :firstChildUpdate_ ( false ) {}

// Destructor
GazeboRosBridgeModelPlugin::~GazeboRosBridgeModelPlugin() {

    //event::Events::DisconnectWorldUpdateBegin ( updateConnection_ );      //DEPRECATED

    pubJointMeasures_.shutdown();
    pubJointsStates_.shutdown();
    pubOdometry_    .shutdown();
    subJointsCmd_   .shutdown();

    queue_.clear();
    queue_.disable();
    alive_ = false;
    callbackQueueThread_.join();
}

// Load the controller
void GazeboRosBridgeModelPlugin::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) {

    loadParameters ( _parent,_sdf );
    loadJoints ();
    setJointsConstraints();

    ///@todo create proper odom message that can init the ICC as well
    body_state_initial_.pose.pose.position.x  = 0;
    body_state_initial_.pose.pose.position.y  = 0;
    body_state_initial_.pose.pose.position.z  = 0;
    body_state_initial_.twist.twist.linear.x  = 0;
    body_state_initial_.twist.twist.linear.y  = 0;
    body_state_initial_.twist.twist.linear.z  = 0;
    body_state_initial_.twist.twist.angular.x = 0;
    body_state_initial_.twist.twist.angular.y = 0;
    body_state_initial_.twist.twist.angular.z = 0;
    initAllStates ( body_state_initial_ );

    // Initialize update rate stuff
    if ( update_rate_ > 0.0 ) {
        update_period_ = 1.0 / update_rate_;
    } else                      {
        update_period_ = 0.0;
    }
    last_update_time_ = parent_->GetWorld()->SimTime();

    //Publishers
    pubJointMeasures_ = gazeboRos_->node()->advertise<tuw_nav_msgs::JointsIWS> ( joints_measures_topic_.c_str(), 1 );
    ROS_INFO ( "%s: Advertise joint_measures!", gazeboRos_->info() );

    if ( publishJointStates_ ) {
        pubJointsStates_ = gazeboRos_->node()->advertise<sensor_msgs::JointState> ( joint_states_topic_.c_str(), 1 );
        ROS_INFO ( "%s: Advertise joint_states!", gazeboRos_->info() );
    }
    if ( publishOdometry_ ) {
        pubOdometry_ = gazeboRos_->node()->advertise<nav_msgs::Odometry> ( odometry_topic_, 1 );
        ROS_INFO ( "%s: Advertise ground-truth odometry on %s !", gazeboRos_->info(), odometry_topic_.c_str() );
    }

    transform_broadcaster_ = boost::shared_ptr<tf::TransformBroadcaster> ( new tf::TransformBroadcaster() );
    if ( publishTfs_ ) {
        ROS_INFO ( "%s: Advertise tf-s!", gazeboRos_->info() );
    }

    //Subscribers
    queue_.enable();

    ROS_INFO ( "%s: Try to subscribe to %s!", gazeboRos_->info(), joints_cmd_topic_.c_str() );
    ros::SubscribeOptions so;
    so = ros::SubscribeOptions::create<tuw_nav_msgs::JointsIWS> ( joints_cmd_topic_, 1, boost::bind ( &GazeboRosBridgeModelPlugin::jointsCmdCallback, this, _1 ), ros::VoidPtr(), &queue_ );
    so.transport_hints = ros::TransportHints().reliable().tcpNoDelay ( true );
    subJointsCmd_ = gazeboRos_->node()->subscribe ( so );
    ROS_INFO ( "%s: Subscribe to %s!", gazeboRos_->info(), joints_cmd_topic_.c_str() );

    // listen to the update event (broadcast every simulation iteration)
    updateConnection_ = event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosBridgeModelPlugin::UpdateChild, this ) );

    //reconfigure stuff
    reconfigureServer_ = std::make_shared< dynamic_reconfigure::Server<tuw_gazebo_plugins::GazeboRosBridgeModelPluginConfig> > ( ros::NodeHandle ( "~/" + gazeboRos_->getPluginName() ) );
    reconfigureFnc_    = boost::bind ( &GazeboRosBridgeModelPlugin::callbackConfig, this,  _1, _2 );
    reconfigureServer_->setCallback ( reconfigureFnc_ );

    // start custom queue
    alive_ = true;
    callbackQueueThread_ = boost::thread ( boost::bind ( &GazeboRosBridgeModelPlugin::QueueThread     , this ) );
}

void GazeboRosBridgeModelPlugin::loadParameters ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) {
    parent_ = _parent;
    gazeboRos_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "TUWBridgeModel", false ) );
    // Make sure the ROS node for Gazebo has already been initialized
    gazeboRos_->isInitialized();

    gazeboRos_->getParameter ( joint_states_topic_,       "joints_states_topic", std::string ( "joint_states" ) );
    gazeboRos_->getParameter ( joints_measures_topic_,     "joints_measures_topic", std::string ( "joints_measures" ) );
    gazeboRos_->getParameter ( joints_cmd_topic_,          "joints_cmd_topic", std::string ( "joints_cmds" ) );
    gazeboRos_->getParameter ( odometry_topic_,                "odom_topic", std::string ( "odom_ground_truth" ) );
    gazeboRos_->getParameter ( odometry_frame_,                "odom_frame", std::string ( "odom_ground_truth" ) );
    gazeboRos_->getParameter ( robot_base_frame_,          "model_base_frame", std::string ( "base_footprint" ) );
    gazeboRos_->getParameterBoolean ( publishTfs_,               "publish_tfs",                            false );
    gazeboRos_->getParameterBoolean ( publishJointStates_,     "publish_joints_states",                            false );
    gazeboRos_->getParameterBoolean ( publishOdometry_, "publish_odom_ground_truth",                             true );

    gazeboRos_->getParameter<double> ( constr_hi_stop_steering_ ,       "steering_hi_stop"   ,                            M_PI );
    gazeboRos_->getParameter<double> ( constr_lo_stop_steering_ ,       "steering_lo_stop"   ,                          - M_PI );
    gazeboRos_->getParameter<double> ( constr_max_torque_[STEER],       "steering_max_torque",                            10.0 );
    gazeboRos_->getParameter<double> ( constr_max_vel_   [STEER],       "steering_max_vel"   ,                            10.0 );
    gazeboRos_->getParameter<double> ( constr_damping_   [STEER],       "steering_damping"   ,                             1.0 );
    gazeboRos_->getParameter<double> ( constr_friction_  [STEER],       "steering_friction"  ,                             0.0 );

    gazeboRos_->getParameter<double> ( constr_max_torque_[REVOL],       "revolute_max_torque",                            10.0 );
    gazeboRos_->getParameter<double> ( constr_max_vel_   [REVOL],       "revolute_max_vel"   ,                            10.0 );
    gazeboRos_->getParameter<double> ( constr_damping_   [REVOL],       "revolute_damping"   ,                             1.0 );
    gazeboRos_->getParameter<double> ( constr_friction_  [REVOL],       "revolute_friction"  ,                             0.0 );

    gazeboRos_->getParameter<double> ( update_rate_,                    "updateRate"         ,                            100.0 );
    gazeboRos_->getParameter<double> ( timeout_cmd_,                    "timeout_cmd"        ,                             10.0 );

    gazeboRos_->getParameter ( jointsMeasureType_[STEER],    "steering_measures_type", std::string ( "measured_position" ) );
    gazeboRos_->getParameter ( jointsMeasureType_[REVOL],    "revolute_measures_type", std::string ( "measured_velocity" ) );

    for ( size_t i = 0; i < jointsMeasureType_.size(); ++i ) {
        if ( !jointsMeasureType_[i].compare ( "measured_position" ) ) {
            getJointMeasure[i] = [] ( physics::JointPtr& _joint ) {
                return _joint->Position ( 0 );
            };
        } else if ( !jointsMeasureType_[i].compare ( "measured_velocity" ) ) {
            getJointMeasure[i] = [] ( physics::JointPtr& _joint ) {
                return _joint->GetVelocity ( 0 );
            };
        } else if ( !jointsMeasureType_[i].compare ( "measured_torque  " ) ) {
            getJointMeasure[i] = [] ( physics::JointPtr& _joint ) {
                return _joint->GetForce ( 0 );
            };
        } else {
            getJointMeasure[i] = [] ( physics::JointPtr& _joint ) {
                return nan ( "" );
            };
            ROS_ERROR ( "%s: Joint measurement mode: \"%s\" is not supported", gazeboRos_->info(), jointsMeasureType_[i].c_str() );
        }
    }
    for ( auto& setJointCmdTypeI : setJointCmd ) {
        setJointCmdTypeI = [] ( physics::JointPtr& _joint, double _cmd ) {};
    }

    jointsCmd_.type_steering = "cmd_position";
    jointsCmd_.type_revolute = "cmd_position";
    setCmdMode ( STEER, jointsCmd_.type_steering );
    setCmdMode ( REVOL, jointsCmd_.type_revolute );

    joints_measure_ = boost::shared_ptr<tuw_nav_msgs::JointsIWS> ( new tuw_nav_msgs::JointsIWS );
    last_cmd_received_ = parent_->GetWorld()->SimTime();
}

void GazeboRosBridgeModelPlugin::setCmdMode ( size_t _jointsTypeIdx, const std::string& _mode ) {
    if ( !jointsCmdType_[_jointsTypeIdx].compare ( _mode ) ) {
        return;
    }

    jointsCmdType_[_jointsTypeIdx] = _mode;
    if ( !_mode.compare ( "cmd_position" ) ) {
        setJointCmd[_jointsTypeIdx] = [] ( physics::JointPtr& _joint, double _cmd ) {
            _joint->SetPosition ( 0, _cmd );
        };
    } else if ( !_mode.compare ( "cmd_velocity" ) ) {
        setJointCmd[_jointsTypeIdx] = [] ( physics::JointPtr& _joint, double _cmd ) {
            /*_joint->SetForce   ( 0, 1 * (_cmd - _joint->GetVelocity(0)) ); ROS_INFO("err=%lf, vel_des=%lf, vel_now=%lf, damping=%lf",_cmd - _joint->GetVelocity(0), _cmd, _joint->GetVelocity(0), _joint->GetDamping(0) );*/
            _joint->SetVelocity ( 0, _cmd );
        };
    } else if ( !_mode.compare ( "cmd_torque  " ) ) {
        setJointCmd[_jointsTypeIdx] = [] ( physics::JointPtr& _joint, double _cmd ) {
            _joint->SetForce ( 0, _cmd );
        };
    } else {
        setJointCmd[_jointsTypeIdx] = [] ( physics::JointPtr& _joint, double _cmd ) {};
        if ( joints_[_jointsTypeIdx].size() > 0 ) {
            ROS_ERROR ( "%s: Joint cmd mode: \"%s\" is not supported", gazeboRos_->info(), _mode.c_str() );
        }
    }
}

void GazeboRosBridgeModelPlugin::loadJoints() {
    sdf::ElementPtr modelSdf = gazeboRos_->Sdf();
    if ( ! ( modelSdf->HasElement ( "steering_joints_names" ) || modelSdf->HasElement ( "revolute_joints_names" ) ) ) {
        ROS_ERROR ( "%s: Loaded SDF contains no steering_joints_names and no revolute_joints_names!", gazeboRos_->info() );
    }
    if ( modelSdf->HasElement ( "steering_joints_names" ) ) {
        joints_[STEER].clear();
        sdf::ElementPtr element = modelSdf->GetElement ( "steering_joints_names" ) ;
        std::vector<string> steerJointsNamesVec;
        std::string steerJointsNames = element->Get<std::string>();
        boost::erase_all ( steerJointsNames, " " );
        boost::split ( steerJointsNamesVec, steerJointsNames, boost::is_any_of ( "," ) );
        for ( size_t i = 0; i < steerJointsNamesVec.size(); ++i ) {
            joints_[STEER].emplace_back ( parent_->GetJoint ( steerJointsNamesVec[i] ) );
            if ( joints_[STEER].back() == nullptr ) {
                ROS_ERROR ( "%s: Name of revolute joint number %lu (%s) not found in model!", gazeboRos_->info(), i, steerJointsNamesVec[i].c_str() );
            }
        }
    }
    if ( modelSdf->HasElement ( "revolute_joints_names" ) ) {
        joints_[REVOL].clear();
        sdf::ElementPtr element = modelSdf->GetElement ( "revolute_joints_names" ) ;
        std::vector<string> revolJointsNamesVec;
        std::string revolJointsNames = element->Get<std::string>();
        boost::erase_all ( revolJointsNames, " " );
        boost::split ( revolJointsNamesVec, revolJointsNames, boost::is_any_of ( "," ) );
        for ( size_t i = 0; i < revolJointsNamesVec.size(); ++i ) {
            joints_[REVOL].emplace_back ( parent_->GetJoint ( revolJointsNamesVec[i] ) );
            if ( joints_[REVOL].back() == nullptr ) {
                ROS_ERROR ( "%s: Name of revolute joint number %lu (%s) not found in model!", gazeboRos_->info(), i, revolJointsNamesVec[i].c_str() );
            }
        }
    }

    jointsCmd_.revolute.resize ( joints_[REVOL].size(), 0 );
    jointsCmd_.steering.resize ( joints_[STEER].size(), 0 );
}

void GazeboRosBridgeModelPlugin::setJointsConstraints() {
    for ( auto& steerJointI : joints_[STEER] ) {
        ignition::math::Angle angle;
        steerJointI->SetUpperLimit ( 0, constr_hi_stop_steering_ );
        steerJointI->SetLowerLimit ( 0, constr_lo_stop_steering_ );
    }
    for ( size_t i = 0; i < joints_.size(); ++i ) {
        for ( auto& JointI : joints_[i] ) {
//       JointI->SetEffortLimit      (            0, constr_max_torque_[i] );
//       JointI->SetVelocityLimit    (            0, constr_max_vel_   [i] );
//       JointI->SetParam            ("friction", 0, constr_damping_   [i] );
//       JointI->SetDamping          (            0, constr_damping_   [i] );
//       JointI->Update();
        }
    }
}

void GazeboRosBridgeModelPlugin::initAllStates ( const nav_msgs::Odometry& body_state ) {
    body_state_ = body_state;
    for ( size_t i = 0; i < joints_.size(); ++i ) {
        for ( auto& JointI : joints_[i] ) {
            JointI->Init();
        }
    }
    double leg_phi = 0;
    for ( auto& steerJointI : joints_[STEER] ) {
        steerJointI->SetPosition ( 0, leg_phi );    ///@todo make it proper
    }
    double leg_v   = 0;
    for ( auto& revolJointI : joints_[REVOL] ) {
        revolJointI->SetVelocity ( 0, leg_v );    ///@todo make it proper
    }
}

// read/write into sim
void GazeboRosBridgeModelPlugin::UpdateChild() {
    if ( !firstChildUpdate_ ) {
        setJointsConstraints();
        firstChildUpdate_ = true;
    }

    common::Time current_time = parent_->GetWorld()->SimTime();
    double seconds_since_last_update = ( current_time - last_update_time_ ).Double();

    if ( seconds_since_last_update > update_period_ ) {
        last_update_time_ = current_time;

        if ( true )                {
            publishJointMeasurements();
        }
        if ( publishOdometry_ )    {
            publishWorldOdometry ( seconds_since_last_update );
        }
        if ( publishTfs_ )         {
            publishTFs();
        }
        if ( publishJointStates_ ) {
            publishJointsStates();
        }
    }
    double seconds_since_last_cmd = (current_time - last_cmd_received_).Double();
    if ( seconds_since_last_cmd > timeout_cmd_ ) {
        ROS_WARN ( "%s joint_cmd timeout %lf sec reached", gazeboRos_->info(), timeout_cmd_ );
        last_cmd_received_ = current_time;
        jointsCmd_.type_steering = "cmd_velocity";
        jointsCmd_.type_revolute = "cmd_velocity";
        setCmdMode ( STEER, jointsCmd_.type_steering );
        setCmdMode ( REVOL, jointsCmd_.type_revolute );
        for ( size_t i = 0; i < jointsCmd_.steering.size(); ++i ) {
            jointsCmd_.steering[i] = 0.;
            ROS_WARN ( "%s jointsCmd_.steering[%zu] = %lf", gazeboRos_->info(), i, jointsCmd_.steering[i]  );
        }
        for ( size_t i = 0; i < jointsCmd_.revolute.size(); ++i ) {
            jointsCmd_.revolute[i]  = 0.;
            ROS_WARN ( "%s jointsCmd_.revolute[%zu] = %lf", gazeboRos_->info(), i, jointsCmd_.revolute[i]  );
        }
    }

    for ( size_t i = 0; i < jointsCmd_.steering.size(); ++i ) {
        setJointCmd[STEER] ( joints_[STEER][i], jointsCmd_.steering[i] );
    }
    for ( size_t i = 0; i < jointsCmd_.revolute.size(); ++i ) {
        setJointCmd[REVOL] ( joints_[REVOL][i], jointsCmd_.revolute[i] );
    }
}

void GazeboRosBridgeModelPlugin::callbackConfig ( tuw_gazebo_plugins::GazeboRosBridgeModelPluginConfig& _config, uint32_t _level ) {
    config_ = _config;
    ROS_DEBUG ( "%s: callbackConfig!", gazeboRos_->info() );
}

void GazeboRosBridgeModelPlugin::jointsCmdCallback ( const tuw_nav_msgs::JointsIWS::ConstPtr& _msgJointsCmd ) {
    last_cmd_received_ = parent_->GetWorld()->SimTime();
    jointsCmd_ = *_msgJointsCmd;
    if ( jointsCmd_.steering.size() != joints_[STEER].size() ) {
        ROS_WARN ( "%s Received a meesage with \"steering\" field size not equal to %lu", gazeboRos_->info(), joints_[STEER].size() );
    }
    if ( jointsCmd_.revolute.size() != joints_[REVOL].size() ) {
        ROS_WARN ( "%s Received a meesage with \"revolute\" field size not equal to %lu", gazeboRos_->info(), joints_[REVOL].size() );
    }
    setCmdMode ( STEER, jointsCmd_.type_steering );
    setCmdMode ( REVOL, jointsCmd_.type_revolute );
}

void GazeboRosBridgeModelPlugin::publishJointsStates() {
    ros::Time current_time = ros::Time::now();

    joint_state_.header.stamp = current_time;
    size_t allJointsSize = 0;
    for ( auto& jointsTypeI : joints_ ) {
        allJointsSize += jointsTypeI.size();
    }
    joint_state_.name.resize ( allJointsSize );
    joint_state_.position.resize ( allJointsSize );
    joint_state_.velocity.resize ( allJointsSize );
    size_t i = 0;
    for ( auto& jointsTypeI : joints_ ) {
        for ( auto& jointI : jointsTypeI ) {
            joint_state_.name    [i] = jointI->GetName ( );
            //joint_state_.position[i] = jointI->GetAngle   ( 0 ).Radian() ;    //DEPRECATED
            joint_state_.position[i] = jointI->Position ( 0 ) ;
            joint_state_.velocity[i] = jointI->GetVelocity ( 0 ) ;
            i++;
        }
    }
    pubJointsStates_.publish ( joint_state_ );
}

void GazeboRosBridgeModelPlugin::publishJointMeasurements() {
    ros::Time current_time = ros::Time::now();

    joints_measure_->header.stamp = current_time;

    joints_measure_->type_revolute = jointsMeasureType_[REVOL];
    joints_measure_->revolute.resize ( joints_[REVOL].size() );
    for ( size_t i = 0; i < joints_[REVOL].size(); ++i ) {
        joints_measure_->revolute[i] = getJointMeasure[REVOL] ( joints_[REVOL][i] );
    }

    joints_measure_->type_steering = jointsMeasureType_[STEER];
    joints_measure_->steering.resize ( joints_[STEER].size() );
    for ( size_t i = 0; i < joints_[STEER].size(); ++i ) {
        joints_measure_->steering[i] = getJointMeasure[STEER] ( joints_[STEER][i] );
    }

    pubJointMeasures_.publish ( joints_measure_ );
}

void GazeboRosBridgeModelPlugin::publishTFs() {
    ros::Time current_time = ros::Time::now();
    size_t i = 0;
    for ( auto& jointsTypeI : joints_ ) {
        for ( auto& jointI : jointsTypeI ) {
            string child_frame  = gazeboRos_->resolveTF ( jointI-> GetChild()->GetName () );
            string parent_frame = gazeboRos_->resolveTF ( jointI->GetParent()->GetName () );

            //math::Pose pose_child   = jointI->GetChild() ->GetRelativePose();//this returns relative pose to parent model! i.e. base link  //DEPRECATED
            ignition::math::Pose3d pose_child = jointI->GetChild()->RelativePose();//this returns relative pose to parent model! i.e. base link
            //math::Pose pose_parrent = jointI->GetParent()->GetRelativePose();                  //DEPRECATED
            ignition::math::Pose3d pose_parrent = jointI->GetParent()->RelativePose();
            //math::Pose pose_rel_to_parrent = pose_child - pose_parrent;                  //DEPRECATED
            ignition::math::Pose3d pose_rel_to_parrent = pose_child - pose_parrent;

            //tf::Quaternion qt ( pose_rel_to_parrent.rot.x, pose_rel_to_parrent.rot.y, pose_rel_to_parrent.rot.z, pose_rel_to_parrent.rot.w );  //DEPRECATED
            tf::Quaternion qt ( pose_rel_to_parrent.Rot().X(),pose_rel_to_parrent.Rot().Y(),pose_rel_to_parrent.Rot().Z(),pose_rel_to_parrent.Rot().W() );
            //tf::Vector3    vt ( pose_rel_to_parrent.pos.x, pose_rel_to_parrent.pos.y, pose_rel_to_parrent.pos.z );        //DEPRECATED
            tf::Vector3    vt ( pose_rel_to_parrent.Pos().X(), pose_rel_to_parrent.Pos().Y(), pose_rel_to_parrent.Pos().Z() );

            tf::Transform tfChild ( qt, vt );
            transform_broadcaster_->sendTransform ( tf::StampedTransform ( tfChild, current_time, parent_frame, child_frame ) );
        }
    }
}

void GazeboRosBridgeModelPlugin::publishWorldOdometry ( double step_time ) {

    ros::Time current_time           = ros::Time::now();
    std::string odom_frame           = gazeboRos_->resolveTF ( odometry_frame_ );
    std::string base_footprint_frame = gazeboRos_->resolveTF ( robot_base_frame_ );

    tf::Quaternion qt;
    tf::Vector3 vt;

    //math::Pose pose = parent_->GetWorldPose();          //DEPRECATED
    ignition::math::Pose3d pose = parent_->WorldPose();

    //getting data form encoder integration
    //qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w );
    qt = tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() );    //DEPRECATED
    //vt = tf::Vector3    ( pose.pos.x, pose.pos.y, pose.pos.z );
    vt = tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() );           //DEPRECATED

//     if (this->publishTfs_) {
    tf::Transform base_footprint_to_odom ( qt, vt );
    transform_broadcaster_->sendTransform ( tf::StampedTransform ( base_footprint_to_odom, current_time, odom_frame, base_footprint_frame ) );
//     }

    nav_msgs::Odometry odom_;

    // set pose position            //DEPRECATED
    odom_.pose.pose.position.x = pose.Pos().X();
    odom_.pose.pose.position.y = pose.Pos().Y();

    odom_.pose.pose.orientation.x = pose.Rot().X();
    odom_.pose.pose.orientation.y = pose.Rot().Y();
    odom_.pose.pose.orientation.z = pose.Rot().Z();
    odom_.pose.pose.orientation.w = pose.Rot().W();
    odom_.pose.covariance[0 ] = 0.00001;
    odom_.pose.covariance[7 ] = 0.00001;
    odom_.pose.covariance[14] = 1000000000000.0;
    odom_.pose.covariance[21] = 1000000000000.0;
    odom_.pose.covariance[28] = 1000000000000.0;
    odom_.pose.covariance[35] = 0.01;

    // set pose velocity
    //math::Vector3 linear;        //DEPRECATED
    ignition::math::Vector3<double> linear;
    linear = parent_->WorldLinearVel();
    odom_.twist.twist.angular.z = parent_->WorldAngularVel().Z();

    // convert velocity to child_frame_id (aka base_footprint)
    //float yaw = pose.rot.GetYaw();            //DEPRECATED
    float yaw = pose.Rot().Yaw();
    odom_.twist.twist.linear.x = cos ( yaw ) * linear.X() + sin ( yaw ) * linear.Y();
    odom_.twist.twist.linear.y = cos ( yaw ) * linear.Y() - sin ( yaw ) * linear.X();

    // set header
    odom_.header.stamp    = current_time;
    odom_.header.frame_id = odom_frame;
    odom_.child_frame_id  = base_footprint_frame;

    pubOdometry_.publish ( odom_ );
}

void GazeboRosBridgeModelPlugin::Init() {
    gazebo::ModelPlugin::Init();
}

void GazeboRosBridgeModelPlugin::Reset() {
    gazebo::ModelPlugin::Reset();
    last_update_time_ = parent_->GetWorld()->SimTime();
    initAllStates ( body_state_initial_ );
}

void GazeboRosBridgeModelPlugin::QueueThread() {
    static const double timeout = 0.01;
    while ( alive_ && gazeboRos_->node()->ok() ) {
        queue_.callAvailable ( ros::WallDuration ( timeout ) );
    }
}

GZ_REGISTER_MODEL_PLUGIN ( GazeboRosBridgeModelPlugin )
}

ext: cpp


text:
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/regex.hpp>
#include <set>
#include <string>
#include <stdexcept>

#if MKL_FOUND
//#include <omp.h>
#include <mkl.h>
#else
#if BLAS_FOUND
//#include <omp.h>
#include <cblas.h>
#endif
#endif

#include "3rd_party/cnpy/cnpy.h"
#include "common/definitions.h"
#include "common/config.h"
#include "common/config_parser.h"
#include "common/file_stream.h"
#include "common/logging.h"
#include "common/version.h"

#define SET_OPTION(key, type)                    \
  do {                                           \
    if(!vm_[key].defaulted() || !config_[key]) { \
      config_[key] = vm_[key].as<type>();        \
    }                                            \
  } while(0)

#define SET_OPTION_NONDEFAULT(key, type)  \
  do {                                    \
    if(vm_.count(key) > 0) {              \
      config_[key] = vm_[key].as<type>(); \
    }                                     \
  } while(0)

namespace po = boost::program_options;

namespace marian {

uint16_t guess_terminal_width(uint16_t max_width) {
  uint16_t cols = 0;
#ifdef TIOCGSIZE
  struct ttysize ts;
  ioctl(STDIN_FILENO, TIOCGSIZE, &ts);
  if(ts.ts_cols != 0)
    cols = ts.ts_cols;
#elif defined(TIOCGWINSZ)
  struct winsize ts;
  ioctl(STDIN_FILENO, TIOCGWINSZ, &ts);
  if(ts.ws_col != 0)
    cols = ts.ws_col;
#endif
  if(cols == 0)  // couldn't determine terminal width
    cols = po::options_description::m_default_line_length;
  return max_width ? std::min(cols, max_width) : cols;
}

void OutputYaml(const YAML::Node node, YAML::Emitter& out) {
  std::set<std::string> sorter;
  switch(node.Type()) {
    case YAML::NodeType::Null: out << node; break;
    case YAML::NodeType::Scalar: out << node; break;
    case YAML::NodeType::Sequence:
      out << YAML::BeginSeq;
      for(auto&& n : node)
        OutputYaml(n, out);
      out << YAML::EndSeq;
      break;
    case YAML::NodeType::Map:
      for(auto& n : node)
        sorter.insert(n.first.as<std::string>());
      out << YAML::BeginMap;
      for(auto& key : sorter) {
        out << YAML::Key;
        out << key;
        out << YAML::Value;
        OutputYaml(node[key], out);
      }
      out << YAML::EndMap;
      break;
    case YAML::NodeType::Undefined: out << node; break;
  }
}

const std::set<std::string> PATHS = {"model",
                                     "models",
                                     "train-sets",
                                     "vocabs",
                                     "embedding-vectors",
                                     "valid-sets",
                                     "valid-script-path",
                                     "valid-log",
                                     "valid-translation-output",
                                     "log"};

void ProcessPaths(YAML::Node& node,
                  const boost::filesystem::path& configPath,
                  bool isPath) {
  using namespace boost::filesystem;

  if(isPath) {
    if(node.Type() == YAML::NodeType::Scalar) {
      std::string nodePath = node.as<std::string>();
      if(nodePath.size()) {
        try {
          node = canonical(path{nodePath}, configPath).string();
        } catch(boost::filesystem::filesystem_error& e) {
          std::cerr << e.what() << std::endl;
          auto parentPath = path{nodePath}.parent_path();
          node = (canonical(parentPath, configPath) / path{nodePath}.filename())
                     .string();
        }
      }
    }

    if(node.Type() == YAML::NodeType::Sequence) {
      for(auto&& sub : node) {
        ProcessPaths(sub, configPath, true);
      }
    }
  } else {
    switch(node.Type()) {
      case YAML::NodeType::Sequence:
        for(auto&& sub : node) {
          ProcessPaths(sub, configPath, false);
        }
        break;
      case YAML::NodeType::Map:
        for(auto&& sub : node) {
          std::string key = sub.first.as<std::string>();
          ProcessPaths(sub.second, configPath, PATHS.count(key) > 0);
        }
        break;
    }
  }
}

bool ConfigParser::has(const std::string& key) const {
  return config_[key];
}

void ConfigParser::validateOptions() const {
  if(mode_ == ConfigMode::translating) {
    UTIL_THROW_IF2(!has("vocabs") || get<std::vector<std::string>>("vocabs").empty(),
        "Translating, but vocabularies are not given!");

    for(const auto& modelFile : get<std::vector<std::string>>("models")) {
      boost::filesystem::path modelPath(modelFile);
      UTIL_THROW_IF2(!boost::filesystem::exists(modelPath),
          "Model file does not exist: " + modelFile);
    }

    return;
  }

  UTIL_THROW_IF2(
      !has("train-sets") || get<std::vector<std::string>>("train-sets").empty(),
      "No train sets given in config file or on command line");
  UTIL_THROW_IF2(
      has("vocabs")
          && get<std::vector<std::string>>("vocabs").size()
                 != get<std::vector<std::string>>("train-sets").size(),
      "There should be as many vocabularies as training sets");
  UTIL_THROW_IF2(
      has("embedding-vectors")
          && get<std::vector<std::string>>("embedding-vectors").size()
                 != get<std::vector<std::string>>("train-sets").size(),
      "There should be as many files with embedding vectors as "
      "training sets");

  boost::filesystem::path modelPath(get<std::string>("model"));

  if(mode_ == ConfigMode::rescoring) {
    UTIL_THROW_IF2(!boost::filesystem::exists(modelPath),
        "Model file does not exist: " + modelPath.string());

    UTIL_THROW_IF2(!has("vocabs") || get<std::vector<std::string>>("vocabs").empty(),
        "Scoring, but vocabularies are not given!");

    return;
  }

  auto modelDir = modelPath.parent_path();
  if(modelDir.empty())
    modelDir = boost::filesystem::current_path();

  UTIL_THROW_IF2(
      !modelDir.empty() && !boost::filesystem::is_directory(modelDir),
      "Model directory does not exist");

  UTIL_THROW_IF2(!modelDir.empty() && !(boost::filesystem::status(modelDir).permissions()
                 & boost::filesystem::owner_write),
                 "No write permission in model directory");

  UTIL_THROW_IF2(
      has("valid-sets")
          && get<std::vector<std::string>>("valid-sets").size()
                 != get<std::vector<std::string>>("train-sets").size(),
      "There should be as many validation sets as training sets");

  // validations for learning rate decaying
  UTIL_THROW_IF2(get<double>("lr-decay") > 1.0,
                 "Learning rate decay factor greater than 1.0 is unusual");
  UTIL_THROW_IF2(
      (get<std::string>("lr-decay-strategy") == "epoch+batches"
       || get<std::string>("lr-decay-strategy") == "epoch+stalled")
          && get<std::vector<size_t>>("lr-decay-start").size() != 2,
      "Decay strategies 'epoch+batches' and 'epoch+stalled' require two "
      "values specified with --lr-decay-start options");
  UTIL_THROW_IF2(
      (get<std::string>("lr-decay-strategy") == "epoch"
       || get<std::string>("lr-decay-strategy") == "batches"
       || get<std::string>("lr-decay-strategy") == "stalled")
          && get<std::vector<size_t>>("lr-decay-start").size() != 1,
      "Single decay strategies require only one value specified with "
      "--lr-decay-start option");
}

void ConfigParser::validateDevices() const {
  std::string devices = Join(get<std::vector<std::string>>("devices"));
  Trim(devices);

  boost::regex pattern;
  std::string help;
  if(mode_ == ConfigMode::training && get<bool>("multi-node")) {
    // valid strings: '0: 1 2', '0:1 2 1:2 3'
    pattern = "( *[0-9]+ *: *[0-9]+( *[0-9]+)*)+";
    help = "Supported format for multi-node setting: '0:0 1 2 3 1:0 1 2 3'";
  } else {
    // valid strings: '0', '0 1 2 3', '3 2 0 1'
    pattern = "[0-9]+( *[0-9]+)*";
    help = "Supported formats: '0 1 2 3'";
  }

  UTIL_THROW_IF2(!boost::regex_match(devices, pattern),
                 "the argument '(" + devices
                     + ")' for option '--devices' is invalid. "
                     + help);
}

void ConfigParser::addOptionsCommon(po::options_description& desc) {
  int defaultWorkspace = (mode_ == ConfigMode::translating) ? 512 : 2048;

  po::options_description general("General options", guess_terminal_width());
  // clang-format off
  general.add_options()
    ("config,c", po::value<std::string>(),
     "Configuration file")
    ("workspace,w", po::value<size_t>()->default_value(defaultWorkspace),
      "Preallocate  arg  MB of work space")
    ("log", po::value<std::string>(),
     "Log training process information to file given by  arg")
    ("log-level", po::value<std::string>()->default_value("info"),
     "Set verbosity level of logging "
     "(trace - debug - info - warn - err(or) - critical - off)")
    ("quiet", po::value<bool>()->zero_tokens()->default_value(false),
     "Suppress all logging to stderr. Logging to files still works")
    ("quiet-translation", po::value<bool>()->zero_tokens()->default_value(false),
     "Suppress logging for translation")
    ("seed", po::value<size_t>()->default_value(0),
     "Seed for all random number generators. 0 means initialize randomly")
    ("relative-paths", po::value<bool>()->zero_tokens()->default_value(false),
     "All paths are relative to the config file location")
    ("dump-config", po::value<bool>()->zero_tokens()->default_value(false),
     "Dump current (modified) configuration to stdout and exit")
    ("version", po::value<bool>()->zero_tokens()->default_value(false),
      "Print version number and exit")
    ("help,h", po::value<bool>()->zero_tokens()->default_value(false),
      "Print this help message and exit")
  ;
  // clang-format on
  desc.add(general);
}

void ConfigParser::addOptionsModel(po::options_description& desc) {
  po::options_description model("Model options", guess_terminal_width());
  // clang-format off
  if(mode_ == ConfigMode::translating) {
    model.add_options()
    ("models,m", po::value<std::vector<std::string>>()->multitoken(),
     "Paths to model(s) to be loaded");
  } else {
    model.add_options()
      ("model,m", po::value<std::string>()->default_value("model.npz"),
      "Path prefix for model to be saved/resumed");

    if(mode_ == ConfigMode::training) {
      model.add_options()
        ("pretrained-model", po::value<std::string>(),
        "Path prefix for pre-trained model to initialize model weights");
    }
  }

  model.add_options()
    ("ignore-model-config", po::value<bool>()->zero_tokens()->default_value(false),
     "Ignore the model configuration saved in npz file")
    ("type", po::value<std::string>()->default_value("amun"),
      "Model type (possible values: amun, nematus, s2s, multi-s2s, transformer)")
    ("dim-vocabs", po::value<std::vector<int>>()
      ->multitoken()
      ->default_value(std::vector<int>({0, 0}), "0 0"),
     "Maximum items in vocabulary ordered by rank, 0 uses all items in the "
     "provided/created vocabulary file")
    ("dim-emb", po::value<int>()->default_value(512),
     "Size of embedding vector")
    ("dim-rnn", po::value<int>()->default_value(1024),
     "Size of rnn hidden state")
    ("enc-type", po::value<std::string>()->default_value("bidirectional"),
     "Type of encoder RNN : bidirectional, bi-unidirectional, alternating (s2s)")
    ("enc-cell", po::value<std::string>()->default_value("gru"),
     "Type of RNN cell: gru, lstm, tanh (s2s)")
    ("enc-cell-depth", po::value<int>()->default_value(1),
     "Number of transitional cells in encoder layers (s2s)")
    ("enc-depth", po::value<int>()->default_value(1),
     "Number of encoder layers (s2s)")
    ("dec-cell", po::value<std::string>()->default_value("gru"),
     "Type of RNN cell: gru, lstm, tanh (s2s)")
    ("dec-cell-base-depth", po::value<int>()->default_value(2),
     "Number of transitional cells in first decoder layer (s2s)")
    ("dec-cell-high-depth", po::value<int>()->default_value(1),
     "Number of transitional cells in next decoder layers (s2s)")
    ("dec-depth", po::value<int>()->default_value(1),
     "Number of decoder layers (s2s)")
    //("dec-high-context", po::value<std::string>()->default_value("none"),
    // "Repeat attended context: none, repeat, conditional, conditional-repeat (s2s)")
    ("skip", po::value<bool>()->zero_tokens()->default_value(false),
     "Use skip connections (s2s)")
    ("layer-normalization", po::value<bool>()->zero_tokens()->default_value(false),
     "Enable layer normalization")
    ("right-left", po::value<bool>()->zero_tokens()->default_value(false),
     "Train right-to-left model")
    ("best-deep", po::value<bool>()->zero_tokens()->default_value(false),
     "Use Edinburgh deep RNN configuration (s2s)")
    ("special-vocab", po::value<std::vector<size_t>>()->multitoken(),
     "Model-specific special vocabulary ids")
    ("tied-embeddings", po::value<bool>()->zero_tokens()->default_value(false),
     "Tie target embeddings and output embeddings in output layer")
    ("tied-embeddings-src", po::value<bool>()->zero_tokens()->default_value(false),
     "Tie source and target embeddings")
    ("tied-embeddings-all", po::value<bool>()->zero_tokens()->default_value(false),
     "Tie all embedding layers and output layer")
    ("transformer-heads", po::value<int>()->default_value(8),
     "Number of head in multi-head attention (transformer)")
    ("transformer-dim-ffn", po::value<int>()->default_value(2048),
     "Size of position-wise feed-forward network (transformer)")
    ("transformer-preprocess", po::value<std::string>()->default_value(""),
     "Operation before each transformer layer: d = dropout, a = add, n = normalize")
    ("transformer-postprocess-emb", po::value<std::string>()->default_value("d"),
     "Operation after transformer embedding layer: d = dropout, a = add, n = normalize")
    ("transformer-postprocess", po::value<std::string>()->default_value("dan"),
     "Operation after each transformer layer: d = dropout, a = add, n = normalize")
#ifdef CUDNN
    ("char-stride", po::value<int>()->default_value(5),
     "Width of max-pooling layer after convolution layer in char-s2s model")
    ("char-highway", po::value<int>()->default_value(4),
     "Number of highway network layers after max-pooling in char-s2s model")
    ("char-conv-filters-num", po::value<std::vector<int>>()
      ->default_value(std::vector<int>({200, 200, 250, 250, 300, 300, 300, 300}),
                                      "200 200 250 250 300 300 300 300")
      ->multitoken(),
     "Numbers of convolution filters of correspoding width in char-s2s model")
    ("char-conv-filters-widths", po::value<std::vector<int>>()
     ->default_value(std::vector<int>({1, 2, 3, 4, 5, 6, 7, 8}), "1 2 3 4 5 6 7 8")
      ->multitoken(),
     "Convolution window widths in char-s2s model")
#endif
    ;

  if(mode_ == ConfigMode::training) {
    model.add_options()
      ("dropout-rnn", po::value<float>()->default_value(0),
       "Scaling dropout along rnn layers and time (0 = no dropout)")
      ("dropout-src", po::value<float>()->default_value(0),
       "Dropout source words (0 = no dropout)")
      ("dropout-trg", po::value<float>()->default_value(0),
       "Dropout target words (0 = no dropout)")
      ("grad-dropping-rate", po::value<float>()->default_value(0),
       "Gradient Dropping rate (0 = no gradient Dropping)")
      ("grad-dropping-momentum", po::value<float>()->default_value(0),
       "Gradient Dropping momentum decay rate (0.0 to 1.0)")
      ("grad-dropping-warmup", po::value<size_t>()->default_value(100),
       "Do not apply gradient dropping for the first arg steps")
      ("transformer-dropout", po::value<float>()->default_value(0),
       "Dropout between transformer layers (0 = no dropout)")
      ("transformer-dropout-attention", po::value<float>()->default_value(0),
       "Dropout for transformer attention (0 = no dropout)")
    ;
  }
  // clang-format on

  desc.add(model);
}

void ConfigParser::addOptionsTraining(po::options_description& desc) {
  po::options_description training("Training options", guess_terminal_width());
  // clang-format off
  training.add_options()
    ("cost-type", po::value<std::string>()->default_value("ce-mean"),
      "Optimization criterion: ce-mean, ce-mean-words, ce-sum, perplexity")
    ("overwrite", po::value<bool>()->zero_tokens()->default_value(false),
      "Overwrite model with following checkpoints")
    ("no-reload", po::value<bool>()->zero_tokens()->default_value(false),
      "Do not load existing model specified in --model arg")
    ("train-sets,t", po::value<std::vector<std::string>>()->multitoken(),
      "Paths to training corpora: source target")
    ("vocabs,v", po::value<std::vector<std::string>>()->multitoken(),
      "Paths to vocabulary files have to correspond to --train-sets. "
      "If this parameter is not supplied we look for vocabulary files "
      "source.{yml,json} and target.{yml,json}. "
      "If these files do not exist they are created")
    ("max-length", po::value<size_t>()->default_value(50),
      "Maximum length of a sentence in a training sentence pair")
    ("max-length-crop", po::value<bool>()->zero_tokens()->default_value(false),
      "Crop a sentence to max-length instead of ommitting it if longer than max-length")
    ("after-epochs,e", po::value<size_t>()->default_value(0),
      "Finish after this many epochs, 0 is infinity")
    ("after-batches", po::value<size_t>()->default_value(0),
      "Finish after this many batch updates, 0 is infinity")
    ("disp-freq", po::value<size_t>()->default_value(1000),
      "Display information every  arg  updates")
    ("save-freq", po::value<size_t>()->default_value(10000),
      "Save model file every  arg  updates")
    ("no-shuffle", po::value<bool>()->zero_tokens()->default_value(false),
      "Skip shuffling of training data before each epoch")
    ("tempdir,T", po::value<std::string>()->default_value("/tmp"),
      "Directory for temporary (shuffled) files and database")
    ("sqlite", po::value<std::string>()->default_value("")->implicit_value("temporary"),
      "Use disk-based sqlite3 database for training corpus storage, default "
      "is temporary with path creates persistent storage")
    ("sqlite-drop", po::value<bool>()->zero_tokens()->default_value(false),
      "Drop existing tables in sqlite3 database")
    ("restore-corpus,r", po::value<bool>()->zero_tokens()->default_value(false),
      "Restore the corpus state for seamless training continuation")
    ("devices,d", po::value<std::vector<std::string>>()
      ->multitoken()
      ->default_value(std::vector<std::string>({"0"}), "0"),
      "GPU ID(s) to use for training")
    ("cpu-threads", po::value<size_t>()->default_value(0)->implicit_value(1),
      "Use CPU-based computation with this many independent threads, 0 means GPU-based computation")
    //("omp-threads", po::value<size_t>()->default_value(1),
    //  "Set number of OpenMP threads for each CPU-based thread")

    ("mini-batch", po::value<int>()->default_value(64),
      "Size of mini-batch used during update")
    ("mini-batch-words", po::value<int>()->default_value(0),
      "Set mini-batch size based on words instead of sentences")
    ("mini-batch-fit", po::value<bool>()->zero_tokens()->default_value(false),
      "Determine mini-batch size automatically based on sentence-length to "
      "fit reserved memory")
    ("mini-batch-fit-step", po::value<size_t>()->default_value(10),
      "Step size for mini-batch-fit statistics")
    ("maxi-batch", po::value<int>()->default_value(100),
      "Number of batches to preload for length-based sorting")
    ("maxi-batch-sort", po::value<std::string>()->default_value("trg"),
      "Sorting strategy for maxi-batch: trg (default) src none")

    ("optimizer,o", po::value<std::string>()->default_value("adam"),
     "Optimization algorithm (possible values: sgd, adagrad, adam")
    ("optimizer-params",  po::value<std::vector<float>>()
       ->multitoken(),
     "Parameters for optimization algorithm, e.g. betas for adam")
    ("optimizer-delay", po::value<size_t>()->default_value(1),
     "SGD update delay, 1 = no delay")
    ("learn-rate,l", po::value<double>()->default_value(0.0001),
     "Learning rate")
    ("lr-decay", po::value<double>()->default_value(0.0),
     "Decay factor for learning rate: lr = lr * arg (0 to disable)")
    ("lr-decay-strategy", po::value<std::string>()->default_value("epoch+stalled"),
     "Strategy for learning rate decaying "
     "(possible values: epoch, batches, stalled, epoch+batches, epoch+stalled)")
    ("lr-decay-start", po::value<std::vector<size_t>>()
       ->multitoken()
       ->default_value(std::vector<size_t>({10,1}), "10 1"),
       "The first number of epoch/batches/stalled validations to start "
       "learning rate decaying")
    ("lr-decay-freq", po::value<size_t>()->default_value(50000),
     "Learning rate decaying frequency for batches, "
     "requires --lr-decay-strategy to be batches")
    ("lr-decay-reset-optimizer", po::value<bool>()->zero_tokens()->default_value(false),
      "Reset running statistics of optimizer whenever learning rate decays")

    ("lr-decay-repeat-warmup", po::value<bool>()
     ->zero_tokens()->default_value(false),
     "Repeat learning rate warmup when learning rate is decayed")
    ("lr-decay-inv-sqrt", po::value<size_t>()->default_value(0),
     "Decrease learning rate at arg / sqrt(no. updates) starting at arg")

    ("lr-warmup", po::value<size_t>()->default_value(0),
     "Increase learning rate linearly for arg first steps")
    ("lr-warmup-start-rate", po::value<float>()->default_value(0),
     "Start value for learning rate warmup")
    ("lr-warmup-cycle", po::value<bool>()->zero_tokens()->default_value(false),
     "Apply cyclic warmup")
    ("lr-warmup-at-reload", po::value<bool>()->zero_tokens()->default_value(false),
     "Repeat warmup after interrupted training")

    ("lr-report", po::value<bool>()
     ->zero_tokens()->default_value(false),
     "Report learning rate for each update")

    ("batch-flexible-lr", po::value<bool>()->zero_tokens()->default_value(false),
      "Scales the learning rate based on the number of words in a mini-batch")
    ("batch-normal-words", po::value<double>()->default_value(1920.0),
      "Set number of words per batch that the learning rate corresponds to. "
      "The option is only active when batch-flexible-lr is on")
    ("sync-sgd", po::value<bool>()->zero_tokens()->default_value(false),
     "Use synchronous SGD instead of asynchronous for multi-gpu training")
    ("label-smoothing", po::value<double>()->default_value(0),
     "Epsilon for label smoothing (0 to disable)")
    ("clip-norm", po::value<double>()->default_value(1.f),
     "Clip gradient norm to  arg  (0 to disable)")
    ("exponential-smoothing", po::value<float>()->default_value(0.f)->implicit_value(1e-4, "1e-4"),
     "Maintain smoothed version of parameters for validation and saving with smoothing factor arg. "
     " 0 to disable.")
    ("guided-alignment", po::value<std::string>(),
     "Use guided alignment to guide attention")
    ("guided-alignment-cost", po::value<std::string>()->default_value("ce"),
     "Cost type for guided alignment. Possible values: ce (cross-entropy), "
     "mse (mean square error), mult (multiplication)")
    ("guided-alignment-weight", po::value<double>()->default_value(1),
     "Weight for guided alignment cost")
    ("data-weighting", po::value<std::string>(),
     "File with sentence or word weights")
    ("data-weighting-type", po::value<std::string>()->default_value("sentence"),
     "Processing level for data weighting. Possible values: sentence, word")

    //("drop-rate", po::value<double>()->default_value(0),
    // "Gradient drop ratio (read: https://arxiv.org/abs/1704.05021)")
    ("embedding-vectors", po::value<std::vector<std::string>>()
      ->multitoken(),
     "Paths to files with custom source and target embedding vectors")
    ("embedding-normalization", po::value<bool>()
      ->zero_tokens()
      ->default_value(false),
     "Enable normalization of custom embedding vectors")
    ("embedding-fix-src", po::value<bool>()
      ->zero_tokens()
      ->default_value(false),
     "Fix source embeddings. Affects all encoders")
    ("embedding-fix-trg", po::value<bool>()
      ->zero_tokens()
      ->default_value(false),
     "Fix target embeddings. Affects all decoders")

    ("multi-node", po::value<bool>()
      ->zero_tokens()
      ->default_value(false),
     "Enable multi-node training through MPI")
    ("multi-node-overlap", po::value<bool>()
      ->default_value(true),
     "Overlap model computations with MPI communication")
  ;
  // clang-format on
  desc.add(training);
}

void ConfigParser::addOptionsValid(po::options_description& desc) {
  po::options_description valid("Validation set options",
                                guess_terminal_width());
  // clang-format off
  valid.add_options()
    ("valid-sets", po::value<std::vector<std::string>>()->multitoken(),
      "Paths to validation corpora: source target")
    ("valid-freq", po::value<size_t>()->default_value(10000),
      "Validate model every  arg  updates")
    ("valid-metrics", po::value<std::vector<std::string>>()
      ->multitoken()
      ->default_value(std::vector<std::string>({"cross-entropy"}),
                      "cross-entropy"),
      "Metric to use during validation: cross-entropy, perplexity, "
      "valid-script, translation. "
      "Multiple metrics can be specified")
    ("valid-mini-batch", po::value<int>()->default_value(32),
      "Size of mini-batch used during validation")
    ("valid-max-length", po::value<size_t>()->default_value(1000),
      "Maximum length of a sentence in a validating sentence pair")
    ("valid-script-path", po::value<std::string>(),
     "Path to external validation script. "
     "It should print a single score to stdout. "
     "If the option is used with validating translation, the output "
     "translation file will be passed as a first argument ")
    ("early-stopping", po::value<size_t>()->default_value(10),
     "Stop if the first validation metric does not improve for  arg  consecutive "
     "validation steps")
    ("keep-best", po::value<bool>()->zero_tokens()->default_value(false),
      "Keep best model for each validation metric")
    ("valid-log", po::value<std::string>(),
     "Log validation scores to file given by  arg")

    ("valid-translation-output", po::value<std::string>(),
     "Path to store the translation")
    ("beam-size,b", po::value<size_t>()->default_value(12),
      "Beam size used during search with validating translator")
    ("normalize,n", po::value<float>()->default_value(0.f)->implicit_value(1.f),
      "Divide translation score by pow(translation length, arg) ")
    ("allow-unk", po::value<bool>()->zero_tokens()->default_value(false),
      "Allow unknown words to appear in output")
    ("n-best", po::value<bool>()->zero_tokens()->default_value(false),
      "Generate n-best list")
  ;
  // clang-format on
  desc.add(valid);
}

void ConfigParser::addOptionsTranslate(po::options_description& desc) {
  po::options_description translate("Translator options",
                                    guess_terminal_width());
  // clang-format off
  translate.add_options()
    ("input,i", po::value<std::vector<std::string>>()
      ->multitoken()
      ->default_value(std::vector<std::string>({"stdin"}), "stdin"),
      "Paths to input file(s), stdin by default")
    ("vocabs,v", po::value<std::vector<std::string>>()->multitoken(),
      "Paths to vocabulary files have to correspond to --input")
    ("beam-size,b", po::value<size_t>()->default_value(12),
      "Beam size used during search")
    ("normalize,n", po::value<float>()->default_value(0.f)->implicit_value(1.f),
      "Divide translation score by pow(translation length, arg) ")
    ("allow-unk", po::value<bool>()->zero_tokens()->default_value(false),
      "Allow unknown words to appear in output")
    ("max-length", po::value<size_t>()->default_value(1000),
      "Maximum length of a sentence in a training sentence pair")
    ("max-length-crop", po::value<bool>()->zero_tokens()->default_value(false),
      "Crop a sentence to max-length instead of ommitting it if longer than max-length")
    ("devices,d", po::value<std::vector<std::string>>()
      ->multitoken()
      ->default_value(std::vector<std::string>({"0"}), "0"),
      "GPUs to use for translating")
    ("cpu-threads", po::value<size_t>()->default_value(0)->implicit_value(1),
      "Use CPU-based computation with this many independent threads, 0 means GPU-based computation")
    //("omp-threads", po::value<size_t>()->default_value(1),
    //  "Set number of OpenMP threads for each CPU-based thread")

    ("mini-batch", po::value<int>()->default_value(1),
      "Size of mini-batch used during update")
    ("maxi-batch", po::value<int>()->default_value(1),
      "Number of batches to preload for length-based sorting")
    ("maxi-batch-sort", po::value<std::string>()->default_value("none"),
      "Sorting strategy for maxi-batch: none (default) src")
    ("n-best", po::value<bool>()->zero_tokens()->default_value(false),
      "Display n-best list")
    //("lexical-table", po::value<std::string>(),
    // "Path to lexical table")
    ("weights", po::value<std::vector<float>>()
      ->multitoken(),
      "Scorer weights")
    // TODO: the options should be available only in server
    ("port,p", po::value<size_t>()->default_value(8080),
      "Port number for web socket server")
  ;
  // clang-format on
  desc.add(translate);
}

void ConfigParser::addOptionsRescore(po::options_description& desc) {
  po::options_description rescore("Rescorer options", guess_terminal_width());
  // clang-format off
  rescore.add_options()
    ("no-reload", po::value<bool>()->zero_tokens()->default_value(false),
      "Do not load existing model specified in --model arg")
    ("train-sets,t", po::value<std::vector<std::string>>()->multitoken(),
      "Paths to corpora to be scored: source target")
    ("vocabs,v", po::value<std::vector<std::string>>()->multitoken(),
      "Paths to vocabulary files have to correspond to --train-sets. "
      "If this parameter is not supplied we look for vocabulary files "
      "source.{yml,json} and target.{yml,json}. "
      "If these files do not exists they are created")
    ("summary", po::value<std::string>()->implicit_value("cross-entropy"),
      "Only print total cost, possible values: cross-entropy (ce-mean), ce-mean-words, ce-sum, perplexity")
    ("max-length", po::value<size_t>()->default_value(1000),
      "Maximum length of a sentence in a training sentence pair")
    ("max-length-crop", po::value<bool>()->zero_tokens()->default_value(false),
      "Crop a sentence to max-length instead of ommitting it if longer than max-length")
    ("devices,d", po::value<std::vector<std::string>>()
      ->multitoken()
      ->default_value(std::vector<std::string>({"0"}), "0"),
      "GPUs to use for training")
    ("cpu-threads", po::value<size_t>()->default_value(0)->implicit_value(1),
      "Use CPU-based computation with this many independent threads, 0 means GPU-based computation")
    //("omp-threads", po::value<size_t>()->default_value(1),
    //  "Set number of OpenMP threads for each CPU-based thread")

    ("mini-batch", po::value<int>()->default_value(64),
      "Size of mini-batch used during update")
    ("mini-batch-words", po::value<int>()->default_value(0),
      "Set mini-batch size based on words instead of sentences")
    ("maxi-batch", po::value<int>()->default_value(100),
      "Number of batches to preload for length-based sorting")
    ("maxi-batch-sort", po::value<std::string>()->default_value("trg"),
      "Sorting strategy for maxi-batch: trg (default) src none")
    ;
  // clang-format on
  desc.add(rescore);
}

void ConfigParser::parseOptions(int argc, char** argv, bool doValidate) {
  addOptionsCommon(cmdline_options_);
  addOptionsModel(cmdline_options_);

  // clang-format off
  switch(mode_) {
    case ConfigMode::translating:
      addOptionsTranslate(cmdline_options_);
      break;
    case ConfigMode::rescoring:
      addOptionsRescore(cmdline_options_);
      break;
    case ConfigMode::training:
      addOptionsTraining(cmdline_options_);
      addOptionsValid(cmdline_options_);
      break;
  }
  // clang-format on

  boost::program_options::variables_map vm_;
  try {
    po::store(
        po::command_line_parser(argc, argv).options(cmdline_options_).run(),
        vm_);
    po::notify(vm_);
  } catch(std::exception& e) {
    std::cerr << "Error: " << e.what() << std::endl << std::endl;
    std::cerr << "Usage: " + std::string(argv[0]) + " [options]" << std::endl;
    std::cerr << cmdline_options_ << std::endl;
    exit(1);
  }

  if(vm_["help"].as<bool>()) {
    std::cerr << "Usage: " + std::string(argv[0]) + " [options]" << std::endl;
    std::cerr << cmdline_options_ << std::endl;
    exit(0);
  }

  // @TODO: move to validateOptions()
  if(mode_ == ConfigMode::translating) {
    if(vm_.count("models") == 0 && vm_.count("config") == 0) {
      std::cerr << "Error: you need to provide at least one model file or a "
                   "config file"
                << std::endl
                << std::endl;

      std::cerr << "Usage: " + std::string(argv[0]) + " [options]" << std::endl;
      std::cerr << cmdline_options_ << std::endl;
      exit(0);
    }
  }

  if(vm_["version"].as<bool>()) {
    std::cerr << PROJECT_VERSION_FULL << std::endl;
    exit(0);
  }

  bool loadConfig = vm_.count("config");
  bool reloadConfig
      = (mode_ == ConfigMode::training)
        && boost::filesystem::exists(vm_["model"].as<std::string>() + ".yml")
        && !vm_["no-reload"].as<bool>();
  std::string configPath;

  if(loadConfig) {
    configPath = vm_["config"].as<std::string>();
    config_ = YAML::Load(InputFileStream(configPath));
  } else if(reloadConfig) {
    configPath = vm_["model"].as<std::string>() + ".yml";
    config_ = YAML::Load(InputFileStream(configPath));
  }

  /** model **/

  if(mode_ == ConfigMode::translating) {
    SET_OPTION_NONDEFAULT("models", std::vector<std::string>);
  } else {
    SET_OPTION("model", std::string);
    if(mode_ == ConfigMode::training) {
      SET_OPTION_NONDEFAULT("pretrained-model", std::string);
    }
  }

  if(!vm_["vocabs"].empty()) {
    config_["vocabs"] = vm_["vocabs"].as<std::vector<std::string>>();
  }

  SET_OPTION("ignore-model-config", bool);
  SET_OPTION("type", std::string);
  SET_OPTION("dim-vocabs", std::vector<int>);
  SET_OPTION("dim-emb", int);
  SET_OPTION("dim-rnn", int);

  SET_OPTION("enc-type", std::string);
  SET_OPTION("enc-cell", std::string);
  SET_OPTION("enc-cell-depth", int);
  SET_OPTION("enc-depth", int);

  SET_OPTION("dec-cell", std::string);
  SET_OPTION("dec-cell-base-depth", int);
  SET_OPTION("dec-cell-high-depth", int);
  SET_OPTION("dec-depth", int);

  SET_OPTION("skip", bool);
  SET_OPTION("tied-embeddings", bool);
  SET_OPTION("tied-embeddings-src", bool);
  SET_OPTION("tied-embeddings-all", bool);
  SET_OPTION("layer-normalization", bool);
  SET_OPTION("right-left", bool);
  SET_OPTION("transformer-heads", int);
  SET_OPTION("transformer-preprocess", std::string);
  SET_OPTION("transformer-postprocess", std::string);
  SET_OPTION("transformer-postprocess-emb", std::string);
  SET_OPTION("transformer-dim-ffn", int);
  SET_OPTION("transformer-dim-ffn", int);

#ifdef CUDNN
  SET_OPTION("char-stride", int);
  SET_OPTION("char-highway", int);
  SET_OPTION("char-conv-filters-num", std::vector<int>);
  SET_OPTION("char-conv-filters-widths", std::vector<int>);
#endif

  SET_OPTION("best-deep", bool);
  SET_OPTION_NONDEFAULT("special-vocab", std::vector<size_t>);

  if(mode_ == ConfigMode::training) {
    SET_OPTION("cost-type", std::string);

    SET_OPTION("dropout-rnn", float);
    SET_OPTION("dropout-src", float);
    SET_OPTION("dropout-trg", float);

    SET_OPTION("grad-dropping-rate", float);
    SET_OPTION("grad-dropping-momentum", float);
    SET_OPTION("grad-dropping-warmup", size_t);

    SET_OPTION("transformer-dropout", float);
    SET_OPTION("transformer-dropout-attention", float);

    SET_OPTION("overwrite", bool);
    SET_OPTION("no-reload", bool);
    if(!vm_["train-sets"].empty()) {
      config_["train-sets"] = vm_["train-sets"].as<std::vector<std::string>>();
    }
    SET_OPTION("after-epochs", size_t);
    SET_OPTION("after-batches", size_t);
    SET_OPTION("disp-freq", size_t);
    SET_OPTION("save-freq", size_t);
    SET_OPTION("no-shuffle", bool);
    SET_OPTION("tempdir", std::string);
    SET_OPTION("sqlite", std::string);
    SET_OPTION("sqlite-drop", bool);
    SET_OPTION("restore-corpus", bool);

    SET_OPTION("optimizer", std::string);
    SET_OPTION_NONDEFAULT("optimizer-params", std::vector<float>);
    SET_OPTION("optimizer-delay", size_t);
    SET_OPTION("learn-rate", double);
    SET_OPTION("sync-sgd", bool);
    SET_OPTION("mini-batch-words", int);
    SET_OPTION("mini-batch-fit", bool);
    SET_OPTION("mini-batch-fit-step", size_t);

    SET_OPTION("lr-decay", double);
    SET_OPTION("lr-decay-strategy", std::string);
    SET_OPTION("lr-decay-start", std::vector<size_t>);
    SET_OPTION("lr-decay-freq", size_t);
    SET_OPTION("lr-decay-reset-optimizer", bool);
    SET_OPTION("lr-warmup", size_t);

    SET_OPTION("lr-decay-inv-sqrt", size_t);
    SET_OPTION("lr-warmup-start-rate", float);
    SET_OPTION("lr-warmup-cycle", bool);

    SET_OPTION("lr-decay-repeat-warmup", bool);
    SET_OPTION("lr-warmup-at-reload", bool);
    SET_OPTION("lr-report", bool);

    SET_OPTION("batch-flexible-lr", bool);
    SET_OPTION("batch-normal-words", double);

    SET_OPTION("label-smoothing", double);
    SET_OPTION("clip-norm", double);
    SET_OPTION("exponential-smoothing", float);

    SET_OPTION_NONDEFAULT("guided-alignment", std::string);
    SET_OPTION("guided-alignment-cost", std::string);
    SET_OPTION("guided-alignment-weight", double);
    SET_OPTION_NONDEFAULT("data-weighting", std::string);
    SET_OPTION("data-weighting-type", std::string);

    // SET_OPTION("drop-rate", double);
    SET_OPTION_NONDEFAULT("embedding-vectors", std::vector<std::string>);
    SET_OPTION("embedding-normalization", bool);
    SET_OPTION("embedding-fix-src", bool);
    SET_OPTION("embedding-fix-trg", bool);

    SET_OPTION("multi-node", bool);
    SET_OPTION("multi-node-overlap", bool);
  }

  if(mode_ == ConfigMode::rescoring) {
    SET_OPTION("no-reload", bool);
    if(!vm_["train-sets"].empty()) {
      config_["train-sets"] = vm_["train-sets"].as<std::vector<std::string>>();
    }
    SET_OPTION("mini-batch-words", int);
    SET_OPTION_NONDEFAULT("summary", std::string);
  }

  if(mode_ == ConfigMode::translating) {
    SET_OPTION("input", std::vector<std::string>);
    SET_OPTION("beam-size", size_t);
    SET_OPTION("normalize", float);
    SET_OPTION("allow-unk", bool);
    SET_OPTION("n-best", bool);
    SET_OPTION_NONDEFAULT("weights", std::vector<float>);
    SET_OPTION("port", size_t);
  }

  /** valid **/

  if(mode_ == ConfigMode::training) {
    if(!vm_["valid-sets"].empty()) {
      config_["valid-sets"] = vm_["valid-sets"].as<std::vector<std::string>>();
    }
    SET_OPTION_NONDEFAULT("valid-sets", std::vector<std::string>);
    SET_OPTION("valid-freq", size_t);
    SET_OPTION("valid-metrics", std::vector<std::string>);
    SET_OPTION("valid-mini-batch", int);
    SET_OPTION("valid-max-length", size_t);
    SET_OPTION_NONDEFAULT("valid-script-path", std::string);
    SET_OPTION("early-stopping", size_t);
    SET_OPTION("keep-best", bool);
    SET_OPTION_NONDEFAULT("valid-log", std::string);

    SET_OPTION_NONDEFAULT("valid-translation-output", std::string);
    SET_OPTION("beam-size", size_t);
    SET_OPTION("normalize", float);
    SET_OPTION("allow-unk", bool);
    SET_OPTION("n-best", bool);
  }

  SET_OPTION("workspace", size_t);
  SET_OPTION("log-level", std::string);
  SET_OPTION("quiet", bool);
  SET_OPTION("quiet-translation", bool);
  SET_OPTION_NONDEFAULT("log", std::string);
  SET_OPTION("seed", size_t);
  SET_OPTION("relative-paths", bool);
  SET_OPTION("devices", std::vector<std::string>);
  SET_OPTION("cpu-threads", size_t);
  //SET_OPTION("omp-threads", size_t);

  SET_OPTION("mini-batch", int);
  SET_OPTION("maxi-batch", int);

  SET_OPTION("maxi-batch-sort", std::string);
  SET_OPTION("max-length", size_t);
  SET_OPTION("max-length-crop", bool);

  if(vm_["best-deep"].as<bool>()) {
    config_["layer-normalization"] = true;
    config_["tied-embeddings"] = true;
    config_["enc-type"] = "alternating";
    config_["enc-cell-depth"] = 2;
    config_["enc-depth"] = 4;
    config_["dec-cell-base-depth"] = 4;
    config_["dec-cell-high-depth"] = 2;
    config_["dec-depth"] = 4;
    config_["skip"] = true;
  }

  if(get<bool>("relative-paths") && !vm_["dump-config"].as<bool>())
    ProcessPaths(
        config_, boost::filesystem::path{configPath}.parent_path(), false);

  if(doValidate) {
    try {
      validateOptions();
      validateDevices();
    } catch(util::Exception& e) {
      std::cerr << "Error: " << e.what() << std::endl << std::endl;

      std::cerr << "Usage: " + std::string(argv[0]) + " [options]" << std::endl;
      std::cerr << cmdline_options_ << std::endl;
      exit(1);
    }
  }

  if(vm_["dump-config"].as<bool>()) {
    YAML::Emitter emit;
    OutputYaml(config_, emit);
    std::cout << emit.c_str() << std::endl;
    exit(0);
  }

// @TODO: this should probably be in processOptionDevices()
//#ifdef BLAS_FOUND
//  //omp_set_num_threads(vm_["omp-threads"].as<size_t>());
//#ifdef MKL_FOUND
//  mkl_set_num_threads(vm_["omp-threads"].as<size_t>());
//#endif
//#endif
}

std::vector<DeviceId> ConfigParser::getDevices() {
  std::vector<DeviceId> devices;

  try {

    std::string devicesStr
        = Join(config_["devices"].as<std::vector<std::string>>());

    if(mode_ == ConfigMode::training && get<bool>("multi-node")) {
      auto parts = Split(devicesStr, ":");
      for(size_t i = 1; i < parts.size(); ++i) {
        std::string part = parts[i];
        Trim(part);
        auto ds = Split(part, " ");
        if(i < parts.size() - 1)
          ds.pop_back();

        // does this make sense?
        devices.push_back({ds.size(), DeviceType::gpu});
        for(auto d : ds)
          devices.push_back({std::stoull(d), DeviceType::gpu});
      }
    } else {
      for(auto d : Split(devicesStr))
        devices.push_back({std::stoull(d), DeviceType::gpu});
    }

    if(config_["cpu-threads"].as<size_t>() > 0) {
      devices.clear();
      for(size_t i = 0; i < config_["cpu-threads"].as<size_t>(); ++i)
      devices.push_back({i, DeviceType::cpu});
    }

  }
  catch(...) {
    ABORT("Problem parsing devices, please report an issue on github");
  }

  return devices;
}

YAML::Node ConfigParser::getConfig() const {
  return config_;
}
}

max_stars_repo_name: tilde-nlp/marian-dev

max_issues_repo_name: tilde-nlp/marian-dev

max_forks_repo_name: tilde-nlp/marian-dev


text:
#include "LaserLocalization.h"
#include "common/feature_utils.h"
#include "common/math_utils.h"
#include "common/nanoflann_pcl.h"
#include "common/ros_utils.h"
#include "common/transform_utils.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/QR>

namespace lidar_slam {

LaserLocalization::LaserLocalization() {}

LaserLocalization::~LaserLocalization() {}

bool LaserLocalization::init(ros::NodeHandle &node,
                             ros::NodeHandle &privateNode) {

  map_frame = "map";
  _isLidarPoseReset = false;
  _lidarPoseReset = Eigen::Isometry3f::Identity();
  if (setup(node, privateNode)) {
    _subInitialPose = node.subscribe(
        "initialpose2", 2, &LaserLocalization::initialPoseHandler2, this);
    _subInitialPose2 = node.subscribe(
        "initialpose", 2, &LaserLocalization::initialPoseHandler, this);

    spin_thread = std::thread(&LaserLocalization::spin, this);

    return true;
  }
  return false;
}

void LaserLocalization::initialPoseHandler2(
    const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) {
  handleInitialPoseMessage(*msg);
}

void LaserLocalization::initialPoseHandler(
    const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) {
  std::string global_frame_id_ = "/map";
  if (msg->header.frame_id == "") {
    ROS_WARN("Received initial pose with empty frame_id.  You should always "
             "supply a frame_id.");
    return;
  }

  Eigen::Quaternionf Qml;
  Qml.x() = msg->pose.pose.orientation.x;
  Qml.y() = msg->pose.pose.orientation.y;
  Qml.z() = msg->pose.pose.orientation.z * -1.0;
  Qml.w() = msg->pose.pose.orientation.w * -1.0;

  Eigen::Vector3f Vml;
  Vml(0) = msg->pose.pose.position.x;
  Vml(1) = msg->pose.pose.position.y;
  Vml(2) = msg->pose.pose.position.z;

  Eigen::Isometry3f Tml = Eigen::Isometry3f::Identity();
  Tml.rotate(Qml);
  Tml.pretranslate(Vml);

  mtx_transform.lock();
  Eigen::Quaternionf quat(Tml.rotation());
  imu_que.reset(Vml, quat);
  ROS_INFO_STREAM("init:\n" << Tml.matrix());
  _isLidarPoseReset = true;
  _lidarPoseReset = Tml;
  mtx_transform.unlock();
  _initialized = true;
}

void LaserLocalization::handleInitialPoseMessage(
    const geometry_msgs::PoseWithCovarianceStamped &msg) {
  std::string global_frame_id_ = "/map";
  if (msg.header.frame_id == "") {
    ROS_WARN("Received initial pose with empty frame_id.  You should always "
             "supply a frame_id.");
    return;
  }

  Eigen::Quaternionf Qml;
  Qml.x() = msg.pose.pose.orientation.x;
  Qml.y() = msg.pose.pose.orientation.y;
  Qml.z() = msg.pose.pose.orientation.z;
  Qml.w() = msg.pose.pose.orientation.w;

  Eigen::Vector3f Vml;
  Vml(0) = msg.pose.pose.position.x;
  Vml(1) = msg.pose.pose.position.y;
  Vml(2) = msg.pose.pose.position.z;

  Eigen::Isometry3f Tml = Eigen::Isometry3f::Identity();
  Tml.rotate(Qml);
  Tml.pretranslate(Vml);

  mtx_transform.lock();
  Eigen::Quaternionf quat(Tml.rotation());
  imu_que.reset(Vml, quat);
  ROS_INFO_STREAM("init:\n" << Tml.matrix());
  _isLidarPoseReset = true;
  _lidarPoseReset = Tml;
  mtx_transform.unlock();
  _initialized = true;
}

void LaserLocalization::spin() {
  ros::Rate rate(500);
  bool status = ros::ok();

  while (status) {
    ros::spinOnce();
    process();
    status = ros::ok();
    rate.sleep();
  }
}

void LaserLocalization::optimizeTransform() {

  if(_dynamicMode)
    _dynamic_feature_map.scanMatchScan(_laserCloudCornerStackDS, _laserCloudSurfStackDS,
                            _lidarMappedNew);
  else
    _feature_map->scanMatchScan(_laserCloudCornerStackDS, _laserCloudSurfStackDS,
                            _lidarMappedNew);
 /*
  _scan_match.scanMatchScan(_laserCloudCornerFromMap, _laserCloudSurfFromMap,
                            _laserCloudCornerStackDS, _laserCloudSurfStackDS,
                            _lidarMappedNew);

*/
}

void LaserLocalization::transformUpdate() {
  mtx_transform.lock();
  if (_isLidarPoseReset) {
    _lidarMappedNew = _lidarPoseReset;
    _isLidarPoseReset = false;
  }
  Eigen::Vector3f pos_last = _lidarMappedLast.translation();
  _lidarMappedLast = _lidarMappedNew;
   _lidarOdomLast = _lidarOdomLastMerged;

  mtx_transform.unlock();

  //imu fusion
  Eigen::Isometry3f lidarPose = _lidarMappedNew;
  Eigen::Isometry3f pre_pos, cor_pos;
  imu_que.predict(_timeLaserOdometryMerged, pre_pos);
  if(!_timeLaserOdometryLast.is_zero()){
    _velocity = (lidarPose.translation() - _lidarPoseLast.translation())/(_timeLaserOdometryMerged-_timeLaserOdometryLast).toSec();
    if(_velocity.norm()>30)
      _velocity = Eigen::Vector3f::Zero();
      Eigen::Vector3f velocity = _velocity;
    imu_que.correct(lidarPose, cor_pos, velocity);
  }
  _lidarPoseLast = lidarPose;
  _timeLaserOdometryLast = _timeLaserOdometryMerged;
}

void LaserLocalization::process() {
  if (!hasNewData() || !_initialized) {
    return;
  }

  reset();

  _inputFrameCount++;
  if (_inputFrameSkip > 0) {
    if (_inputFrameCount % (_inputFrameSkip + 1) != 1)
      return;
  }

  transformMerge();
  prepareFeatureFrame();
  prepareFeatureSurround();
  optimizeTransform();
  transformUpdate();
  // featureMapUpdate();
  publishResult();
}

} // end namespace lidar_slam

max_stars_repo_name: autonomy-lab-cooper/CooperMapper

max_issues_repo_name: autonomy-lab-cooper/CooperMapper

max_forks_repo_name: autonomy-lab-cooper/CooperMapper