Merge branch 'xisco' into nestor
This commit is contained in:
commit
f1733c6c76
|
@ -27,6 +27,17 @@ namespace carla {
|
||||||
uint8_t A;
|
uint8_t A;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
enum ImageType{
|
||||||
|
IMAGE,
|
||||||
|
DEPTH,
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Image {
|
||||||
|
std::vector<Color> image;
|
||||||
|
ImageType type;
|
||||||
|
uint32_t width, height;
|
||||||
|
};
|
||||||
|
|
||||||
struct Reward_Values {
|
struct Reward_Values {
|
||||||
Reward_Values();
|
Reward_Values();
|
||||||
~Reward_Values();
|
~Reward_Values();
|
||||||
|
@ -51,13 +62,13 @@ namespace carla {
|
||||||
/// Percentage of the car off-road.
|
/// Percentage of the car off-road.
|
||||||
float intersect_offroad;
|
float intersect_offroad;
|
||||||
/// Width and height of the images.
|
/// Width and height of the images.
|
||||||
uint32_t image_width, image_height;
|
//uint32_t image_width, image_height;
|
||||||
/// RGB images.
|
/// RGB images.
|
||||||
std::vector<Color> image_rgb_0;
|
std::vector<Image> images;
|
||||||
std::vector<Color> image_rgb_1;
|
//std::vector<Color> image_rgb_1;
|
||||||
/// Depth images.
|
/// Depth images.
|
||||||
std::vector<Color> image_depth_0;
|
//std::vector<Color> image_depth_0;
|
||||||
std::vector<Color> image_depth_1;
|
//std::vector<Color> image_depth_1;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct Scene_Values {
|
struct Scene_Values {
|
||||||
|
|
|
@ -17,40 +17,105 @@
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace server {
|
namespace server {
|
||||||
|
|
||||||
|
|
||||||
#ifdef WITH_TURBOJPEG
|
#ifdef WITH_TURBOJPEG
|
||||||
|
|
||||||
static bool getJPEGImage(
|
template<typename Type>
|
||||||
|
static std::string GetBytes(Type n) {
|
||||||
|
|
||||||
|
union{
|
||||||
|
Type num;
|
||||||
|
unsigned char bytes[4];
|
||||||
|
} value;
|
||||||
|
|
||||||
|
value.num = n;
|
||||||
|
|
||||||
|
std::string out_bytes;
|
||||||
|
for (int i = 0 ; i < 4 ; ++i) out_bytes += value.bytes[i];
|
||||||
|
|
||||||
|
return out_bytes;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
static bool getJPEGImage(
|
||||||
const int jpeg_quality,
|
const int jpeg_quality,
|
||||||
const size_t width,
|
const size_t width,
|
||||||
const size_t height,
|
const size_t height,
|
||||||
const std::vector<Color> &image,
|
const std::vector<Color> &image,
|
||||||
bool depth,
|
ImageType image_type,
|
||||||
Reward &rwd){
|
Reward &rwd){
|
||||||
long unsigned int jpegSize = 0;
|
*/
|
||||||
unsigned char *compressedImage;
|
|
||||||
if (image.empty())
|
static bool GetImage(const int jpeg_quality, const Image &image_info, unsigned char **compressedImage, long unsigned int &jpegSize){
|
||||||
|
if (image_info.image.empty())
|
||||||
return false;
|
return false;
|
||||||
if (image.size() != width * height) {
|
if (image_info.image.size() != image_info.width * image_info.height) {
|
||||||
std::cerr << "Invalid image size" << std::endl;
|
std::cerr << "Invalid image size" << std::endl;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// Convert to char array RGBA.
|
|
||||||
std::vector<unsigned char> color_image;
|
std::vector<unsigned char> color_image;
|
||||||
color_image.reserve(3u * image.size());
|
color_image.reserve(3u * image_info.image.size());
|
||||||
for (const Color &color : image) {
|
for (const Color &color : image_info.image) {
|
||||||
color_image.push_back(color.R);
|
color_image.push_back(color.R);
|
||||||
color_image.push_back(color.G);
|
color_image.push_back(color.G);
|
||||||
color_image.push_back(color.B);
|
color_image.push_back(color.B);
|
||||||
}
|
}
|
||||||
|
|
||||||
tjhandle jpegCompressor = tjInitCompress();
|
tjhandle jpegCompressor = tjInitCompress();
|
||||||
tjCompress2(jpegCompressor, color_image.data(), width, 0, height, TJPF_RGB,
|
tjCompress2(jpegCompressor, color_image.data(), image_info.width, 0, image_info.height, TJPF_RGB,
|
||||||
&compressedImage, &jpegSize, TJSAMP_444, jpeg_quality, TJFLAG_FASTDCT);
|
compressedImage, &jpegSize, TJSAMP_444, jpeg_quality, TJFLAG_FASTDCT);
|
||||||
tjDestroy(jpegCompressor);
|
tjDestroy(jpegCompressor);
|
||||||
if (!depth) {
|
|
||||||
rwd.set_image(compressedImage, jpegSize);
|
return true;
|
||||||
} else {
|
}
|
||||||
rwd.set_depth(compressedImage, jpegSize);
|
|
||||||
|
|
||||||
|
static bool getJPEGImages(const int jpeg_quality, const std::vector<Image> &images, Reward &rwd){
|
||||||
|
|
||||||
|
std::string image_data;
|
||||||
|
std::string depth_data;
|
||||||
|
std::string image_size_data;
|
||||||
|
std::string depth_size_data;
|
||||||
|
//nt images_count = 0, depth_count = 0;
|
||||||
|
|
||||||
|
for (const Image &img : images){
|
||||||
|
unsigned char *compressedImage;
|
||||||
|
long unsigned int jpegSize = 0;
|
||||||
|
if (!GetImage(jpeg_quality, img, &compressedImage, jpegSize)) {
|
||||||
|
std::cerr << "Error while encoding image" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
switch (img.type){
|
||||||
|
|
||||||
|
case IMAGE:
|
||||||
|
|
||||||
|
for (unsigned long int i = 0; i < jpegSize; ++i) image_data += compressedImage[i];
|
||||||
|
image_size_data += GetBytes(jpegSize);
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case DEPTH:
|
||||||
|
for (unsigned long int i = 0; i < jpegSize; ++i) depth_data += compressedImage[i];
|
||||||
|
depth_size_data += GetBytes(jpegSize);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
std::cout << "send depth size: " << depth_size_data.size() <<
|
||||||
|
" send image size: " << image_size_data.size()<<
|
||||||
|
" send image: " << image_data.size()<<
|
||||||
|
" send depth: " << depth_data.size() << std::endl;
|
||||||
|
rwd.set_depth_sizes(depth_size_data);
|
||||||
|
rwd.set_image_sizes(image_size_data);
|
||||||
|
rwd.set_images(image_data);
|
||||||
|
rwd.set_depths(depth_data);
|
||||||
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -80,57 +145,59 @@ namespace server {
|
||||||
reward.set_timestamp(values.timestamp);
|
reward.set_timestamp(values.timestamp);
|
||||||
|
|
||||||
#ifdef WITH_TURBOJPEG
|
#ifdef WITH_TURBOJPEG
|
||||||
// Compress images to JPEG
|
|
||||||
|
|
||||||
struct ImageHolder {
|
|
||||||
bool isDepth;
|
|
||||||
const decltype(values.image_rgb_0) ℑ
|
|
||||||
};
|
|
||||||
|
|
||||||
std::vector<ImageHolder> images;
|
|
||||||
if (_communication->GetMode() == Mode::MONO) {
|
|
||||||
images.push_back({false, values.image_rgb_0});
|
|
||||||
} else if (_communication->GetMode() == Mode::STEREO) {
|
|
||||||
images.reserve(4u);
|
|
||||||
images.push_back({false, values.image_rgb_0});
|
|
||||||
images.push_back({false, values.image_rgb_1});
|
|
||||||
images.push_back({true, values.image_depth_0});
|
|
||||||
images.push_back({true, values.image_depth_1});
|
|
||||||
} else {
|
|
||||||
std::cerr << "Error, invalid mode" << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
constexpr int JPEG_QUALITY = 75;
|
constexpr int JPEG_QUALITY = 75;
|
||||||
for (const auto &holder : images) {
|
|
||||||
if (!getJPEGImage(
|
if (!getJPEGImages(JPEG_QUALITY, values.images, reward)) {
|
||||||
JPEG_QUALITY,
|
|
||||||
values.image_width,
|
|
||||||
values.image_height,
|
|
||||||
holder.image,
|
|
||||||
holder.isDepth,
|
|
||||||
reward)) {
|
|
||||||
std::cerr << "Error compressing image to JPEG" << std::endl;
|
std::cerr << "Error compressing image to JPEG" << std::endl;
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // WITH_TURBOJPEG
|
#endif // WITH_TURBOJPEG
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Protocol::LoadScene(Scene &scene, const Scene_Values &values) {
|
void Protocol::LoadScene(Scene &scene, const Scene_Values &values) {
|
||||||
for (auto i = 0u; i < values.possible_positions.size(); ++i) {
|
|
||||||
Scene::Position *point = scene.add_position();
|
|
||||||
point->set_pos_x(values.possible_positions[i].x);
|
|
||||||
point->set_pos_y(values.possible_positions[i].y);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_communication->GetMode() == Mode::STEREO) {
|
scene.set_number_of_cameras(values.projection_matrices.size());
|
||||||
for (auto i = 0u; i < values.projection_matrices.size(); ++i) {
|
|
||||||
Scene::Projection_Matrix *matrix = scene.add_camera_matrix();
|
std::string positions_bytes = "";
|
||||||
for (auto e = 0u; e < 16u; ++e) {
|
|
||||||
matrix->add_cam_param(values.projection_matrices[i][e]);
|
std::cout << "------- POSITIONS--------" << std::endl;
|
||||||
}
|
|
||||||
|
|
||||||
|
for (auto i = 0u; i < values.possible_positions.size(); ++i) {
|
||||||
|
|
||||||
|
float x = values.possible_positions[i].x;
|
||||||
|
float y = values.possible_positions[i].y;
|
||||||
|
|
||||||
|
positions_bytes += GetBytes(x) + GetBytes(y);
|
||||||
|
|
||||||
|
std::cout << "x: " << x << " byte size: " << sizeof(float) << " bytes: " << GetBytes(x) << std::endl;
|
||||||
|
std::cout << "y: " << y << " byte size: " << sizeof(float) << " bytes: " << GetBytes(y) << std::endl;
|
||||||
|
|
||||||
|
}
|
||||||
|
std::cout << "---------------" << std::endl;
|
||||||
|
std::cout << "Final string: "<< positions_bytes << std::endl;
|
||||||
|
std::cout << "---------------" << std::endl;
|
||||||
|
|
||||||
|
scene.set_positions(positions_bytes);
|
||||||
|
|
||||||
|
std::string matrices;
|
||||||
|
|
||||||
|
for (auto i = 0u; i < values.projection_matrices.size(); ++i) {
|
||||||
|
for (auto e = 0u; e < 16u; ++e){
|
||||||
|
|
||||||
|
float value = values.projection_matrices[i][e];
|
||||||
|
|
||||||
|
char data [sizeof(float)];
|
||||||
|
memcpy(data, &value, sizeof value);
|
||||||
|
matrices.append(data);
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
scene.set_projection_matrices(matrices);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Protocol::LoadWorld(World &world, const int modes, const int scenes) {
|
void Protocol::LoadWorld(World &world, const int modes, const int scenes) {
|
||||||
|
|
|
@ -10,7 +10,7 @@ namespace carla {
|
||||||
|
|
||||||
using boost::asio::ip::tcp;
|
using boost::asio::ip::tcp;
|
||||||
|
|
||||||
std::string GetBytes(int n) {
|
static std::string GetBytes(int n) {
|
||||||
std::string bytes;
|
std::string bytes;
|
||||||
|
|
||||||
bytes = (n >> 24) & 0xFF;
|
bytes = (n >> 24) & 0xFF;
|
||||||
|
@ -21,7 +21,7 @@ namespace carla {
|
||||||
return bytes;
|
return bytes;
|
||||||
}
|
}
|
||||||
|
|
||||||
int GetInt(char b1, char b2, char b3, char b4) {
|
static int GetInt(char b1, char b2, char b3, char b4) {
|
||||||
int result = 0;
|
int result = 0;
|
||||||
result = (result << 8) + b1;
|
result = (result << 8) + b1;
|
||||||
result = (result << 8) + b2;
|
result = (result << 8) + b2;
|
||||||
|
|
|
@ -50,27 +50,9 @@ message World {
|
||||||
|
|
||||||
message Scene {
|
message Scene {
|
||||||
|
|
||||||
|
optional int32 number_of_cameras = 1;
|
||||||
|
optional bytes positions = 2;
|
||||||
message Projection_Matrix{
|
optional bytes projection_matrices = 3;
|
||||||
|
|
||||||
repeated float cam_param = 1;
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
message Position{
|
|
||||||
|
|
||||||
required float pos_x = 1;
|
|
||||||
required float pos_y = 2;
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
repeated Position position =1;
|
|
||||||
repeated Projection_Matrix camera_matrix =2;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -100,6 +82,9 @@ message Reward {
|
||||||
optional float ori_x = 13;
|
optional float ori_x = 13;
|
||||||
optional float ori_y = 14;
|
optional float ori_y = 14;
|
||||||
optional float ori_z = 15;
|
optional float ori_z = 15;
|
||||||
optional bytes image = 16;
|
optional bytes image_sizes = 16;
|
||||||
optional bytes depth = 17;
|
optional bytes depth_sizes = 17;
|
||||||
|
optional bytes images = 18;
|
||||||
|
optional bytes depths = 19;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -59,13 +59,22 @@ std::unique_ptr<carla::Reward_Values> makeReward(){
|
||||||
reward->collision_car = 10.0f;
|
reward->collision_car = 10.0f;
|
||||||
reward->intersect_other_lane = 0.5f;
|
reward->intersect_other_lane = 0.5f;
|
||||||
reward->intersect_offroad = 0.5f;
|
reward->intersect_offroad = 0.5f;
|
||||||
reward->image_width = imageWidth;
|
|
||||||
reward->image_height = imageHeight;
|
for (int i = 0; i < 4; ++i){
|
||||||
|
carla::Image img;
|
||||||
|
img.image = makeImage(imageWidth, imageHeight);
|
||||||
|
img.width = imageWidth;
|
||||||
|
img.height = imageHeight;
|
||||||
|
if (i < 2) img.type = carla::IMAGE;
|
||||||
|
else img.type = carla::DEPTH;
|
||||||
|
reward->images.push_back(img);
|
||||||
|
}
|
||||||
|
/*
|
||||||
reward->image_rgb_0 = makeImage(imageWidth, imageHeight);
|
reward->image_rgb_0 = makeImage(imageWidth, imageHeight);
|
||||||
reward->image_rgb_1 = makeImage(imageWidth, imageHeight);
|
reward->image_rgb_1 = makeImage(imageWidth, imageHeight);
|
||||||
reward->image_depth_0 = makeImage(imageWidth, imageHeight);
|
reward->image_depth_0 = makeImage(imageWidth, imageHeight);
|
||||||
reward->image_depth_1 = makeImage(imageWidth, imageHeight);
|
reward->image_depth_1 = makeImage(imageWidth, imageHeight);
|
||||||
|
*/
|
||||||
static decltype(carla::Reward_Values::timestamp) timestamp = 0u;
|
static decltype(carla::Reward_Values::timestamp) timestamp = 0u;
|
||||||
reward->timestamp = timestamp++;
|
reward->timestamp = timestamp++;
|
||||||
|
|
||||||
|
@ -187,8 +196,6 @@ int main(int argc, char *argv[]) {
|
||||||
std::cout << "CONTROL --> gas: " << gas << " steer: " << steer << std::endl;
|
std::cout << "CONTROL --> gas: " << gas << " steer: " << steer << std::endl;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (!server.sendReward(makeReward().release())) {
|
if (!server.sendReward(makeReward().release())) {
|
||||||
std::cerr << "ERROR while sending Reward" << std::endl;
|
std::cerr << "ERROR while sending Reward" << std::endl;
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Reference in New Issue