DRInterface.hh

Go to the documentation of this file.

/*
 *  Copyright (C) 2018 João Borrego
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *  
 *      http://www.apache.org/licenses/LICENSE-2.0
 *      
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#ifndef _DOMAIN_RANDOMIZATION_INTERFACE_HH_
#define _DOMAIN_RANDOMIZATION_INTERFACE_HH_

// Gazebo
#include <gazebo/gazebo_client.hh>
#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

#include <gazebo/physics/bullet/BulletSurfaceParams.hh>
#include <gazebo/physics/ode/ODESurfaceParams.hh>
#include <gazebo/physics/dart/DARTSurfaceParams.hh>

// Custom messages
#include "dr_request.pb.h"
#include "model_cmd.pb.h"

// Debug streams
#include "debug.hh"

// Required fields workaround
#include <limits>

typedef gap::msgs::DRRequest DRRequest;
typedef gap::msgs::ModelCmd ModelCmdMsg;

/*
typedef boost::shared_ptr <gazebo::physics::BulletSurfaceParams>
    BulletSurfaceParamsPtr;
typedef boost::shared_ptr <gazebo::physics::DARTSurfaceParams>
    DARTSurfaceParamsPtr;
typedef boost::shared_ptr <gazebo::physics::ODESurfaceParams>
    ODESurfaceParamsPtr;
*/

class DRInterface
{

    public: static const char REQUEST_TOPIC[];
    public: static const char RESPONSE_TOPIC[];
    public: static const char VISUAL_TOPIC[];
    public: static const int POSITION;
    public: static const int VELOCITY;

    private: gazebo::transport::NodePtr node;
    private: gazebo::transport::PublisherPtr pub;
    private: gazebo::transport::PublisherPtr pub_visual;

    public: DRInterface();

    public: DRRequest createRequest();

    public: void publish(const DRRequest & msg);

    public: void publish(const gazebo::msgs::Visual & msg);   

    // Features

    public: void addGravity(DRRequest & msg,
        const ignition::math::Vector3d & gravity);

    public: void addModelScale(DRRequest & msg,
        const std::string & model,
        const ignition::math::Vector3d & scale);

    public: void addLinkMass(DRRequest & msg,
        const std::string & model,
        const std::string & link,
        double mass);

    public: void addInertiaMatrix(DRRequest & msg,
        const std::string & model,
        const std::string & link,
        double ixx, double iyy, double izz,
        double ixy, double ixz, double iyz);

    public: void addSurface(DRRequest & msg,
        const std::string & model,
        const std::string & link,
        const std::string & collision,
        const gazebo::msgs::Surface & surface);

    public: void addJoint(DRRequest & msg,
        const std::string & model,
        const std::string & joint,
        double limit_lower = INFINITY,
        double limit_upper = INFINITY,
        double limit_effort = INFINITY,
        double limit_velocity = INFINITY,
        double damping = INFINITY,
        double friction = INFINITY);

    public: void addModelCmd(DRRequest & msg,
        const std::string & model,
        const std::string & joint,
        int type,
        double p_gain = INFINITY,
        double i_gain = INFINITY,
        double d_gain = INFINITY);

    public: void addColors(gazebo::msgs::Visual & msg,
        const std::string & visual,
        const std::string & parent,
        const ignition::math::Color & ambient,
        const ignition::math::Color & diffuse,
        const ignition::math::Color & emissive,
        const ignition::math::Color & specular);
};

#endif