WorldUtils.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.
 */

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

// Mutex
#include <mutex>

#include <iostream>
#include <iomanip>
#include <sstream>
#include <list>
#include <string>
#include <regex>
// Boost
#include <boost/algorithm/string/predicate.hpp>
#include <boost/lexical_cast.hpp>

// Custom messages
#include "world_utils_request.pb.h"
#include "world_utils_response.pb.h"

// Õbjects with pending move operations
#include "MoveObject.hh"

namespace WorldUtils {

#define REQUEST_TOPIC "~/gap/world_utils"
#define RESPONSE_TOPIC "~/gap/world_utils/response"

// Ease of use macros

// Request

#define SPAWN           gap::msgs::WorldUtilsRequest::SPAWN
#define MOVE            gap::msgs::WorldUtilsRequest::MOVE
#define REMOVE          gap::msgs::WorldUtilsRequest::REMOVE
#define PHYSICS         gap::msgs::WorldUtilsRequest::PHYSICS
#define PAUSE           gap::msgs::WorldUtilsRequest::PAUSE
#define STATUS          gap::msgs::WorldUtilsRequest::STATUS

#define SPHERE          gap::msgs::Object::SPHERE
#define CYLINDER        gap::msgs::Object::CYLINDER
#define BOX             gap::msgs::Object::BOX
#define CUSTOM          gap::msgs::Object::CUSTOM
#define CUSTOM_LIGHT    gap::msgs::Object::CUSTOM_LIGHT
#define MODEL           gap::msgs::Object::MODEL

// Response

#define INFO            gap::msgs::WorldUtilsResponse::INFO
#define SUCCESS         gap::msgs::WorldUtilsResponse::SUCCESS

// Regex patterns

// Do not raise documentation warning

#define REGEX_XML_SCRIPT "<script>[\\s\\S]*?<\\/script>"
#define REGEX_XML_POSE   "<pose>[\\s\\S]*?<\\/pose>"


}

namespace gazebo {

    typedef const boost::shared_ptr<const gap::msgs::WorldUtilsRequest>
        WorldUtilsRequestPtr;
    typedef const boost::shared_ptr<const gap::msgs::WorldUtilsResponse>
        WorldUtilsResponsePtr;

    class WorldUtils : public WorldPlugin {

        public: std::mutex mutex;

        private: physics::WorldPtr world;
        private: event::ConnectionPtr updateConnection;

        private: transport::NodePtr node;
        private: transport::SubscriberPtr sub;
        private: transport::PublisherPtr pub;

        private: transport::PublisherPtr request_pub;
        private: transport::SubscriberPtr response_sub;
        private: transport::PublisherPtr light_pub;

        // Regex patterns

        private: std::regex script_reg;
        private: std::regex pose_reg;

        // Counters for automatic naming

        private: int sphere_counter      {0};
        private: int cylinder_counter    {0};
        private: int box_counter         {0};
        private: int light_counter       {0};

        private: std::queue<MoveObject> move_queue;

        // Public methods

        public: WorldUtils();

        public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);

        public: void onUpdate();

        // Private methods

        private: void onRequest(WorldUtilsRequestPtr &_msg);

        private: void clearWorld();

        private: void clearMatching(const std::string & match, const bool is_light);

        private: const std::string genSphere(
            const std::string &model_name,
            const double mass,
            const double radius,
            const ignition::math::Vector3d position,
            const ignition::math::Quaterniond orientation);

        private: const std::string genCylinder(
            const std::string &model_name,
            const double mass,
            const double radius,
            const double length,
            const ignition::math::Vector3d position,
            const ignition::math::Quaterniond orientation);

        private: const std::string genBox(
            const std::string &model_name,
            const double mass,
            const ignition::math::Vector3d size,
            const ignition::math::Vector3d position,
            const ignition::math::Quaterniond orientation);
    };
}