MILDRobotModel.hpp

Go to the documentation of this file.

#pragma once

#include <boost/tuple/tuple.hpp>
#include "robot_model_services/robot_model/RobotModel.hpp"
#include "robot_model_services/typedef.hpp"
#include "geometry_msgs/Pose.h"
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include "robot_model_services/helper/DebugHelper.hpp"
#include "robot_model_services/helper/MapHelper.hpp"

namespace robot_model_services {
    class MILDRobotModel : public RobotModel {

    protected:

        DebugHelperPtr mDebugHelperPtr;

                // weighting values
                float mOmegaPan;
                float mOmegaTilt;
        float mOmegaRot;
                float speedFactorPTU;
                float speedFactorBaseMove;
                float speedFactorBaseRot;
                float tolerance;
        float mSigma;
                bool useGlobalPlanner;
        tf::TransformListener listener;
        MapHelperPtr mMapHelperPtr;
        
        ros::NodeHandle n;

        std::string mFrameName_map;
        std::string mFrameName_mild_base;
        std::string mFrameName_mild_ptu_base_link;
        std::string mFrameName_mild_ptu_pan_link;
        std::string mFrameName_mild_ptu_pan_link_rotated;
        std::string mFrameName_mild_ptu_tilt_link;
        std::string mFrameName_mild_ptu_tilt_link_rotated;
        std::string mFrameName_mild_camera_mount_link;
        std::string mFrameName_mild_camera_left;
        std::string mFrameName_mild_camera_right;

                boost::tuple<float, float> mPanLimits;

                boost::tuple<float, float> mTiltLimits;

                boost::tuple<float, float> mRotationLimits;
                
                ros::ServiceClient navigationCostClient;
                
        public:
        MILDRobotModel();

        virtual ~MILDRobotModel();

         nav_msgs::Path getNavigationPath(const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition, double sourceRotationBase, double targetRotationBase);
         nav_msgs::Path getNavigationPath(const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition);

                void setPanAngleLimits(float minAngleDegrees, float maxAngleDegrees);

                void setTiltAngleLimits(float minAngleDegrees, float maxAngleDegrees);

                void setRotationAngleLimits(float minAngleDegrees, float maxAngleDegrees);

        bool isPositionAllowed(const geometry_msgs::Point &position);

                bool isPoseReachable(const SimpleVector3 &position, const SimpleQuaternion &orientation);
                
                bool isPositionReachable(const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition);

        float getBase_TranslationalMovementCosts(const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState);

        float getPTU_TiltMovementCosts(const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState);

        float getPTU_PanMovementCosts(const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState);

        float getBase_RotationalMovementCosts(const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState);
                
                float getDistance(const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition);

        //Returns the current pose of the robots base from tf
        geometry_msgs::Pose getRobotPose();
        //Returns the current pose of the robots camera from tf
        geometry_msgs::Pose getCameraPose();

        geometry_msgs::Pose calculateCameraPose(const RobotStatePtr &sourceRobotState);

    private:
        void initMapHelper();
        };

    typedef boost::shared_ptr<MILDRobotModel> MILDRobotModelPtr;
}