Merge branch 'xisco' into nestor

This commit is contained in:
nsubiron 2017-04-19 12:45:13 +02:00
commit f1733c6c76
5 changed files with 160 additions and 90 deletions

View File

@ -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 {

View File

@ -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) &image;
};
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) {

View File

@ -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;

View File

@ -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;
} }

View File

@ -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;