BaseTest.h

Go to the documentation of this file.

#define BOOST_TEST_STATIC_LINK

#include <ros/ros.h>
#include <boost/array.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <glpk.h>
#include <map>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <dynamic_reconfigure/Reconfigure.h>

#include "asr_msgs/AsrAttributedPoint.h"

#include "robot_model_services/helper/MathHelper.hpp"
#include "robot_model_services/helper/MapHelper.hpp"
#include "robot_model_services/helper/TypeHelper.hpp"
#include "robot_model_services/robot_model/impl/MILDRobotModel.hpp"
#include "robot_model_services/robot_model/impl/MILDRobotModelWithExactIK.hpp"
#include "robot_model_services/robot_model/impl/MILDRobotState.hpp"
#include <tf/tf.h>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <ros/master.h>

using namespace robot_model_services;

class BaseTest {
protected:
    boost::shared_ptr<ros::NodeHandle> mNodeHandle;
    ros::Publisher mInitPosePub;
    bool silent;
public:
    BaseTest();

    BaseTest(bool silent);

    ~BaseTest();

    void init(bool silent);

    void initRosServices();


    robot_model_services::MILDRobotStatePtr getRobotState(const geometry_msgs::Pose &initialPose);

    void waitForEnter();

    SimpleQuaternion euler2Quaternion( const Precision roll, const Precision pitch, const Precision yaw);

    SimpleQuaternion ZXZ2Quaternion( const Precision roll, const Precision pitch, const Precision yaw);

    template<typename T> bool getParameter(const std::string &key, T &parameter)
    {
        if (!mNodeHandle->getParam(key, parameter))
        {
            ROS_ERROR_STREAM(key << " parameter not set!");
            return false;
        }
        else
        {
            return true;
        }
    }
};