TargetPlugin.hh

Go to the documentation of this file.

#ifndef _TARGET_PLUGIN_HH_
#define _TARGET_PLUGIN_HH_

// Gazebo
#include "gazebo/common/Plugin.hh"
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>

// Custom messages
#include "target_request.pb.h"
#include "target_response.pb.h"

namespace TargetPlugin {

    // Plugin messages

    #define REQUEST_TOPIC   "~/grasp/target"
    #define RESPONSE_TOPIC  "~/grasp/target/response"

    #define REQ_GET_POSE        grasp::msgs::TargetRequest::GET_POSE
    #define REQ_SET_POSE        grasp::msgs::TargetRequest::SET_POSE
    #define REQ_GET_REST_POSE   grasp::msgs::TargetRequest::GET_REST_POSE
    #define REQ_RESET           grasp::msgs::TargetRequest::RESET
    #define RES_POSE            grasp::msgs::TargetResponse::POSE
    #define RES_REST_POSE       grasp::msgs::TargetResponse::REST_POSE

    #define LIN_VEL_EPSILON 0.000001
    #define ANG_VEL_EPSILON 0.00001
}

namespace gazebo {

    typedef grasp::msgs::TargetRequest TargetRequest;
    typedef const boost::shared_ptr<const grasp::msgs::TargetRequest>
        TargetRequestPtr;
    typedef grasp::msgs::TargetResponse TargetResponse;
    typedef const boost::shared_ptr<const grasp::msgs::TargetResponse>
        TargetResponsePtr;

    // Forward declaration of private data class
    class TargetPluginPrivate;

    class TargetPlugin : public ModelPlugin
    {
        // Private attributes

        private: std::unique_ptr<TargetPluginPrivate> data_ptr;
        private: physics::ModelPtr model;
        private: event::ConnectionPtr update_connection;
        private: event::ConnectionPtr reset_connection;

        private: bool get_pose {false};
        private: bool set_pose {false};
        private: bool update_rest_pose {false};
        private: bool reset {false};
        private: ignition::math::Pose3d init_pose;
        private: ignition::math::Pose3d new_pose;

        // Public methods

        public: TargetPlugin();

        public: virtual ~TargetPlugin() override;

        public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

        public: void onUpdate();

        public: void onReset();

        public: void onRequest(TargetRequestPtr & _msg);

    };
}

#endif