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 "next_best_view/NextBestView.hpp"
#include "asr_next_best_view/GetNextBestView.h"
#include "asr_next_best_view/SetAttributedPointCloud.h"
#include "asr_next_best_view/SetInitRobotState.h"
#include "asr_next_best_view/ResetCalculator.h"
#include "asr_next_best_view/UpdatePointCloud.h"
#include "asr_next_best_view/TriggerFrustumVisualization.h"
#include "asr_msgs/AsrAttributedPoint.h"

#include "next_best_view/helper/MathHelper.hpp"
#include "next_best_view/helper/MarkerHelper.hpp"
#include "next_best_view/helper/MapHelper.hpp"
#include "next_best_view/helper/ObjectHelper.h"
#include "next_best_view/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 next_best_view;

class BaseTest {
protected:
    boost::shared_ptr<ros::NodeHandle> mNodeHandle;
    ros::Publisher mInitPosePub;
    ros::ServiceClient mSetInitRobotStateClient;
    ros::ServiceClient mSetPointCloudClient;
    ros::ServiceClient mGetPointCloudClient;
    ros::ServiceClient mGetNextBestViewClient;
    ros::ServiceClient mUpdatePointCloudClient;
    ros::ServiceClient mResetCalculatorClient;
    ros::ServiceClient mDynParametersClient;
    ros::ServiceClient mTriggerFrustumVisClient;
    bool silent;
public:
    BaseTest();

    BaseTest(bool useRos, bool silent);

    ~BaseTest();

    void init(bool useRos, bool silent);

    void initRosServices();

    void setInitialPose(const geometry_msgs::Pose &initialPose, boost::shared_ptr<NextBestView> nbv = nullptr);

    void setInitialRobotState(const geometry_msgs::Pose &initialPose, boost::shared_ptr<NextBestView> nbv = nullptr);

    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;
        }
    }
};