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

// Includes

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

// Custom messages
#include "camera_utils_request.pb.h"
#include "camera_utils_response.pb.h"
#include "visual_utils_request.pb.h"
#include "visual_utils_response.pb.h"
#include "world_utils_request.pb.h"
#include "world_utils_response.pb.h"

// Utilities
#include "utils.hh"
// Object class
#include "ObjectGrid.hh"

// I/O streams
#include <iostream>
// File streams
#include <fstream>
// Iterating over the contents of a dir
#include <boost/filesystem.hpp>
// Protecting variables
#include <mutex>
// Set container
#include <set>
// Sleep
#include <chrono>
#include <thread>
// Regular expressions
#include <regex>
// Linear algebra
#include <Eigen/Dense>
// INT MAX
#include <climits>


// Macros

// Fixed viewpoint
#define FIXED_VIEW 0
// Moving viewpoint, change in position
#define MOVING_VIEW_POS 1
// Moving viewpoint, change in position and rotation
#define MOVING_VIEW_POS_ROT 2

// Do not raise documentation error 

#define REGEX_XML_MODEL "<model name=(\"([^\"]|\"\")*\")>"
#define REGEX_XML_UID   "<uid>[\\s\\S]*?<\\/uid>"



// Macros for custom messages

// Camera utils

#define MOVE_REQUEST            gap::msgs::CameraUtilsRequest::MOVE
#define MOVE_RESPONSE           gap::msgs::CameraUtilsResponse::MOVE
#define CAPTURE_REQUEST         gap::msgs::CameraUtilsRequest::CAPTURE
#define CAPTURE_RESPONSE        gap::msgs::CameraUtilsResponse::CAPTURE
#define PROJECTION_REQUEST      gap::msgs::CameraUtilsRequest::PROJECTION
#define PROJECTION_RESPONSE     gap::msgs::CameraUtilsResponse::PROJECTION
#define OPTIONS                 gap::msgs::CameraUtilsRequest::OPTIONS

// Visual utils

#define UPDATE      gap::msgs::VisualUtilsRequest::UPDATE
#define UPDATED     gap::msgs::VisualUtilsResponse::UPDATED

// World utils

#define SPAWN           gap::msgs::WorldUtilsRequest::SPAWN
#define WORLD_MOVE      gap::msgs::WorldUtilsRequest::MOVE
#define PHYSICS         gap::msgs::WorldUtilsRequest::PHYSICS

#define CUSTOM          gap::msgs::Object::CUSTOM
#define CUSTOM_LIGHT    gap::msgs::Object::CUSTOM_LIGHT


// API Topics

#define CAMERA_UTILS_TOPIC          "~/gap/camera_utils"
#define CAMERA_UTILS_RESPONSE_TOPIC "~/gap/camera_utils/response"
#define VISUAL_UTILS_TOPIC          "~/gap/visual_utils"
#define VISUAL_UTILS_RESPONSE_TOPIC "~/gap/visual_utils/response"
#define WORLD_UTILS_TOPIC           "~/gap/world_utils"
#define WORLD_UTILS_RESPONSE_TOPIC  "~/gap/world_utils/response"

// Message pointer typedefs

typedef const boost::shared_ptr<const gap::msgs::CameraUtilsResponse>
    CameraUtilsResponsePtr;
typedef const boost::shared_ptr<const gap::msgs::VisualUtilsResponse>
    VisualUtilsResponsePtr;
typedef const boost::shared_ptr<const gap::msgs::WorldUtilsResponse>
    WorldUtilsResponsePtr;



void addModelFromFile(
    gap::msgs::WorldUtilsRequest & msg,
    const std::string & file);

void addDynamicModels(gap::msgs::WorldUtilsRequest & msg);

void updateObjects(gap::msgs::VisualUtilsRequest & msg);

void addMoveObject(
    gap::msgs::WorldUtilsRequest & msg,
    const std::string & name,
    const bool is_light,
    const ignition::math::Pose3d & pose);

ignition::math::Pose3d getRandomCameraPose();

ignition::math::Pose3d getRandomLightPose();

void captureScene(gazebo::transport::PublisherPtr pub, int iteration);

bool waitForMove();

bool waitForVisuals();

bool waitForCamera();

bool waitForProjections();

void createNameSet();

void addProjections(gap::msgs::CameraUtilsRequest & msg);

void moveCamera(gazebo::transport::PublisherPtr pub);

void onCameraUtilsResponse(CameraUtilsResponsePtr & _msg);

void onVisualUtilsResponse(VisualUtilsResponsePtr & _msg);

void onWorldUtilsResponse(WorldUtilsResponsePtr & _msg);

void setPhysics(gazebo::transport::PublisherPtr pub, bool enable);

void visualizeData(const std::string & image_dir, int iteration);

void storeAnnotations(
    const std::string & path,
    const int iteration);