148 lines
4.8 KiB
C++
148 lines
4.8 KiB
C++
#include <iostream>
|
|
#include <random>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <tuple>
|
|
|
|
#include <carla/client/ActorBlueprint.h>
|
|
#include <carla/client/BlueprintLibrary.h>
|
|
#include <carla/client/Client.h>
|
|
#include <carla/client/Map.h>
|
|
#include <carla/client/Sensor.h>
|
|
#include <carla/client/TimeoutException.h>
|
|
#include <carla/client/World.h>
|
|
#include <carla/geom/Transform.h>
|
|
#include <carla/image/ImageIO.h>
|
|
#include <carla/image/ImageView.h>
|
|
#include <carla/sensor/data/Image.h>
|
|
|
|
namespace cc = carla::client;
|
|
namespace cg = carla::geom;
|
|
namespace csd = carla::sensor::data;
|
|
|
|
using namespace std::chrono_literals;
|
|
using namespace std::string_literals;
|
|
|
|
#define EXPECT_TRUE(pred) if (!(pred)) { throw std::runtime_error(#pred); }
|
|
|
|
/// Pick a random element from @a range.
|
|
template <typename RangeT, typename RNG>
|
|
static auto &RandomChoice(const RangeT &range, RNG &&generator) {
|
|
EXPECT_TRUE(range.size() > 0u);
|
|
std::uniform_int_distribution<size_t> dist{0u, range.size() - 1u};
|
|
return range[dist(std::forward<RNG>(generator))];
|
|
}
|
|
|
|
/// Save a semantic segmentation image to disk converting to CityScapes palette.
|
|
static void SaveSemSegImageToDisk(const csd::Image &image) {
|
|
using namespace carla::image;
|
|
|
|
char buffer[9u];
|
|
std::snprintf(buffer, sizeof(buffer), "%08zu", image.GetFrame());
|
|
auto filename = "_images/"s + buffer + ".png";
|
|
|
|
auto view = ImageView::MakeColorConvertedView(
|
|
ImageView::MakeView(image),
|
|
ColorConverter::CityScapesPalette());
|
|
ImageIO::WriteView(filename, view);
|
|
}
|
|
|
|
static auto ParseArguments(int argc, const char *argv[]) {
|
|
EXPECT_TRUE((argc == 1u) || (argc == 3u));
|
|
using ResultType = std::tuple<std::string, uint16_t>;
|
|
return argc == 3u ?
|
|
ResultType{argv[1u], std::stoi(argv[2u])} :
|
|
ResultType{"localhost", 2000u};
|
|
}
|
|
|
|
int main(int argc, const char *argv[]) {
|
|
try {
|
|
|
|
std::string host;
|
|
uint16_t port;
|
|
std::tie(host, port) = ParseArguments(argc, argv);
|
|
|
|
std::mt19937_64 rng((std::random_device())());
|
|
|
|
auto client = cc::Client(host, port);
|
|
client.SetTimeout(10s);
|
|
|
|
std::cout << "Client API version : " << client.GetClientVersion() << '\n';
|
|
std::cout << "Server API version : " << client.GetServerVersion() << '\n';
|
|
|
|
// Load a random town.
|
|
auto town_name = RandomChoice(client.GetAvailableMaps(), rng);
|
|
std::cout << "Loading world: " << town_name << std::endl;
|
|
auto world = client.LoadWorld(town_name);
|
|
|
|
// Get a random vehicle blueprint.
|
|
auto blueprint_library = world.GetBlueprintLibrary();
|
|
auto vehicles = blueprint_library->Filter("vehicle");
|
|
auto blueprint = RandomChoice(*vehicles, rng);
|
|
|
|
// Randomize the blueprint.
|
|
if (blueprint.ContainsAttribute("color")) {
|
|
auto &attribute = blueprint.GetAttribute("color");
|
|
blueprint.SetAttribute(
|
|
"color",
|
|
RandomChoice(attribute.GetRecommendedValues(), rng));
|
|
}
|
|
|
|
// Find a valid spawn point.
|
|
auto map = world.GetMap();
|
|
auto transform = RandomChoice(map->GetRecommendedSpawnPoints(), rng);
|
|
|
|
// Spawn the vehicle.
|
|
auto actor = world.SpawnActor(blueprint, transform);
|
|
std::cout << "Spawned " << actor->GetDisplayId() << '\n';
|
|
auto vehicle = boost::static_pointer_cast<cc::Vehicle>(actor);
|
|
|
|
// Apply control to vehicle.
|
|
cc::Vehicle::Control control;
|
|
control.throttle = 1.0f;
|
|
vehicle->ApplyControl(control);
|
|
|
|
// Move spectator so we can see the vehicle from the simulator window.
|
|
auto spectator = world.GetSpectator();
|
|
transform.location += 32.0f * transform.GetForwardVector();
|
|
transform.location.z += 2.0f;
|
|
transform.rotation.yaw += 180.0f;
|
|
transform.rotation.pitch = -15.0f;
|
|
spectator->SetTransform(transform);
|
|
|
|
// Find a camera blueprint.
|
|
auto camera_bp = blueprint_library->Find("sensor.camera.semantic_segmentation");
|
|
EXPECT_TRUE(camera_bp != nullptr);
|
|
|
|
// Spawn a camera attached to the vehicle.
|
|
auto camera_transform = cg::Transform{
|
|
cg::Location{-5.5f, 0.0f, 2.8f}, // x, y, z.
|
|
cg::Rotation{-15.0f, 0.0f, 0.0f}}; // pitch, yaw, roll.
|
|
auto cam_actor = world.SpawnActor(*camera_bp, camera_transform, actor.get());
|
|
auto camera = boost::static_pointer_cast<cc::Sensor>(cam_actor);
|
|
|
|
// Register a callback to save images to disk.
|
|
camera->Listen([](auto data) {
|
|
auto image = boost::static_pointer_cast<csd::Image>(data);
|
|
EXPECT_TRUE(image != nullptr);
|
|
SaveSemSegImageToDisk(*image);
|
|
});
|
|
|
|
std::this_thread::sleep_for(10s);
|
|
|
|
// Remove actors from the simulation.
|
|
camera->Destroy();
|
|
vehicle->Destroy();
|
|
std::cout << "Actors destroyed." << std::endl;
|
|
|
|
} catch (const cc::TimeoutException &e) {
|
|
std::cout << '\n' << e.what() << std::endl;
|
|
return 1;
|
|
} catch (const std::exception &e) {
|
|
std::cout << "\nException: " << e.what() << std::endl;
|
|
return 2;
|
|
}
|
|
}
|