Task.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */


#ifndef VISP_TASK_TASK_HPP
#define VISP_TASK_TASK_HPP

#include "visp/TaskBase.hpp"
#include <visp/vpCameraParameters.h>
#include <visp/vpServo.h>
#include <visp/vpPoint.h>
#include <visp/vpPose.h>
#include <visp/vpFeaturePoint.h>
#include <visp/vpFeatureDepth.h>
#include <visp/vpFeatureThetaU.h>
#include <visp/vpFeatureTranslation.h>
#include <apriltags/apriltagsTypes.hpp>


namespace visp{

    typedef apriltags::VisualFeaturePoint Points;
    typedef std::vector<apriltags::VisualFeaturePoint> PointsVector;

    class Task : public TaskBase
    {
        friend class TaskBase;
        protected:

        visp::expectedInputs expected_inputs;
        visp::controllerState ctrl_state;
        base::LinearAngular6DCommand setpoint;

        vpCameraParameters cam_cal;
        vpServo task;
        vpPoint point[4];
        vpPoint P;

        vpHomogeneousMatrix cMb;  //body pose in the camera frame

        //features
        vpFeatureDepth depth;
        vpFeaturePoint p;
        vpFeaturePoint pd;
        vpFeatureThetaU tu;
        vpFeatureTranslation t;
        double Zd;

        //target parameters
        std::string desired_target;
        double rotation_around_z;
        std::vector<visp::targetObjectParameters> target_list;

        vpMatrix eJe;
        vpVelocityTwistMatrix cVb; //transform the velocities of the camera to the body frame


        void updateFeaturesHVS(Points const &corners, vpHomogeneousMatrix const &cdMo);

        vpHomogeneousMatrix updateDesiredPose(base::LinearAngular6DCommand setpoint);

        void setGain(double gain, visp::adaptiveGains const &adaptive_gains, bool use_adaptive);

        void writeVelocities(vpColVector const &v);

        base::samples::RigidBodyState convertToRbs(vpHomogeneousMatrix pose) const;

        vpMatrix jacobianFromExpectedInputs(const visp::expectedInputs &expected_inputs) const;


        bool readSetpoint(base::LinearAngular6DCommand& setpoint);

        bool readCorners(Points& corners, std::string const &desired_target);

        void updateTargetParameters(std::string const &desired_target);

        void setObjectSize(double object_width, double object_height);

        virtual bool setDesired_target(std::string const &value);

        public:
        Task(std::string const& name = "visp::Task");

        Task(std::string const& name, RTT::ExecutionEngine* engine);

        ~Task();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
    };
}

#endif