From 3d67456c184063b6e3899198f4b6fc43a237ca2a Mon Sep 17 00:00:00 2001 From: gerni17 <43470810+gerni17@users.noreply.github.com> Date: Sun, 28 Feb 2021 13:46:37 +0100 Subject: [PATCH 1/7] Gian/testing event sensor (#3) * workin pipeline, wrong velocity format and of doesn't seem to work * nice pipeline with histo, but rather slow * check the framerate * not working pipeline * working pipeline of firt event sensor * commit before implementing trajectory * able to send the events to client * able to transfrm it ina image but not properly working * wrong deserialization * working pipeline * added timestep * not working trajectory * not working * working but not moving * some working * racing is workin properly, the oter one not really * working in racing * first working pipeline, but not ood yet * not working pieline * working but wrong error * not workin testing * having big errors again * events and other thing do not have same coordinates * working with second last images * nothing important * check coroutines * added client evet settings * i don't undertand why but this works * added histogram and removed second image * rosbag publisher * not working * working pipeline * tested and storing rosbags * cleaned code and added recording script * new env, writing to rosbag, no pos wirting * saving image and events * save image * writing images and erasing zeros * proper time function * saving properly * huge error accumulation * little faster and testing is more independent, overall working * recieving all images * snaga * changed testin bag destination * changed to previous port numbers * added depth and int64, still ring trajectory * added snaga config * this looks nice for now * working but has to be cleaned and made smarter, still having the helper function * wrkin ad clean * for snaga to produce video * trajectory * higher res * slightl change of trajectory * recording definitive * major changes to make it adaptive * formatted * added linear depth * added adaptive time of elia * add rotation * added corect pos output * added speed variing * added trajectory * doubled vel * added new trajectory * added angle to pure translation * add fourth traj * trajectory five * sixth trajectory * seventh trajectory * cleaned code --- .gitignore | 2 + .../flightlib/bridges/unity_bridge.hpp | 10 +- .../flightlib/bridges/unity_message_types.hpp | 84 ++- flightlib/include/flightlib/common/types.hpp | 59 +++ .../include/flightlib/objects/quadrotor.hpp | 6 + .../flightlib/sensors/event_camera.hpp | 113 +++++ flightlib/package.xml | 2 +- flightlib/src/bridges/unity_bridge.cpp | 248 +++++++-- flightlib/src/objects/quadrotor.cpp | 20 + flightlib/src/sensors/event_camera.cpp | 253 +++++++++ flightros/CMakeLists.txt | 47 +- flightros/dependencies.yaml | 28 + flightros/include/flightros/flight_pilot.hpp | 70 --- flightros/include/flightros/record.hpp | 90 ++++ flightros/include/flightros/ros_utils.hpp | 40 ++ flightros/include/flightros/rosbag_writer.hpp | 44 ++ flightros/include/flightros/testing.hpp | 94 ++++ flightros/launch/record.launch | 19 + flightros/launch/rotors_gazebo.launch | 122 ----- flightros/launch/rotors_gazebo_test.launch | 11 - flightros/launch/testing.launch | 18 + flightros/package.xml | 18 +- flightros/params/snaga.yaml | 326 ++++++++++++ flightros/src/flight_pilot.cpp | 104 ---- flightros/src/flight_pilot_node.cpp | 13 - flightros/src/record.cpp | 478 ++++++++++++++++++ flightros/src/ros_utils.cpp | 50 ++ flightros/src/rosbag_writer.cpp | 145 ++++++ flightros/src/test/testing.cpp | 392 ++++++++++++++ 29 files changed, 2510 insertions(+), 396 deletions(-) create mode 100644 flightlib/include/flightlib/sensors/event_camera.hpp create mode 100644 flightlib/src/sensors/event_camera.cpp delete mode 100644 flightros/include/flightros/flight_pilot.hpp create mode 100644 flightros/include/flightros/record.hpp create mode 100644 flightros/include/flightros/ros_utils.hpp create mode 100644 flightros/include/flightros/rosbag_writer.hpp create mode 100644 flightros/include/flightros/testing.hpp create mode 100644 flightros/launch/record.launch delete mode 100644 flightros/launch/rotors_gazebo.launch delete mode 100644 flightros/launch/rotors_gazebo_test.launch create mode 100644 flightros/launch/testing.launch create mode 100644 flightros/params/snaga.yaml delete mode 100644 flightros/src/flight_pilot.cpp delete mode 100644 flightros/src/flight_pilot_node.cpp create mode 100644 flightros/src/record.cpp create mode 100644 flightros/src/ros_utils.cpp create mode 100644 flightros/src/rosbag_writer.cpp create mode 100644 flightros/src/test/testing.cpp diff --git a/.gitignore b/.gitignore index babc707ef0..6585482aa1 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,5 @@ dist *.so __pycache__ build + +flightros/params/local.yaml diff --git a/flightlib/include/flightlib/bridges/unity_bridge.hpp b/flightlib/include/flightlib/bridges/unity_bridge.hpp index 0870036bc7..4f39bb9170 100644 --- a/flightlib/include/flightlib/bridges/unity_bridge.hpp +++ b/flightlib/include/flightlib/bridges/unity_bridge.hpp @@ -6,6 +6,7 @@ // std libs #include + #include #include #include @@ -26,8 +27,10 @@ #include "flightlib/objects/quadrotor.hpp" #include "flightlib/objects/static_object.hpp" #include "flightlib/objects/unity_camera.hpp" +#include "flightlib/sensors/event_camera.hpp" #include "flightlib/sensors/rgb_camera.hpp" + using json = nlohmann::json; namespace flightlib { @@ -44,7 +47,9 @@ class UnityBridge { // public get functions bool getRender(const FrameID frame_id); + bool trigggerEvents(const FrameID frame_id); bool handleOutput(); + bool handleOutput(bool always); bool getPointCloud(PointCloudMessage_t &pointcloud_msg, Scalar time_out = 600.0); @@ -69,13 +74,14 @@ class UnityBridge { private: bool initializeConnections(void); - // SettingsMessage_t settings_; PubMessage_t pub_msg_; Logger logger_{"UnityBridge"}; std::vector> unity_quadrotors_; std::vector> rgb_cameras_; + std::vector> event_cameras_; + std::vector> static_objects_; // ZMQ variables and functions @@ -95,7 +101,7 @@ class UnityBridge { int64_t u_packet_latency_; // axuiliary variables - const Scalar unity_connection_time_out_{10.0}; + const Scalar unity_connection_time_out_{1000.0}; bool unity_ready_{false}; }; } // namespace flightlib \ No newline at end of file diff --git a/flightlib/include/flightlib/bridges/unity_message_types.hpp b/flightlib/include/flightlib/bridges/unity_message_types.hpp index d9f58401b4..713d14a088 100644 --- a/flightlib/include/flightlib/bridges/unity_message_types.hpp +++ b/flightlib/include/flightlib/bridges/unity_message_types.hpp @@ -26,9 +26,14 @@ enum UnityScene { TUNELS = 2, NATUREFOREST = 3, // total number of environment - SceneNum = 4 + JAPAN = 4, + SceneNum = 5 }; + +struct EventsMessage_t { + std::vector events; +}; // Unity Camera, should not be used alone. // has to be attached on a vehicle. struct Camera_t { @@ -50,6 +55,30 @@ struct Camera_t { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; }; +struct EventCamera_t { + std::string ID; + // frame Metadata + int channels{3}; + int width{346}; + int height{260}; + Scalar fov{70.0f}; + Scalar depth_scale{0.20}; // 0.xx corresponds to xx cm resolution + // metadata + bool is_depth{false}; + int output_index{0}; + // eventcamera settings + float Cm{0.1}; + float Cp{0.1}; + float sigma_Cp{0}; + float sigma_Cm{0}; + uint64_t refractory_period_ns{0}; + float log_eps{0.0001}; + // Transformation matrix from camera to vehicle body 4 x 4 + // use 1-D vector for json convention + std::vector T_BC{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; +}; + struct Lidar_t { std::string ID; int num_beams{10}; @@ -72,6 +101,7 @@ struct Vehicle_t { std::vector size{1.0, 1.0, 1.0}; // scale // sensors attached on the vehicle std::vector cameras; + std::vector eventcameras; std::vector lidars; // collision check bool has_collision_check = true; @@ -91,8 +121,6 @@ struct Object_t { struct SettingsMessage_t { // scene/render settings size_t scene_id = UnityScene::WAREHOUSE; - - // std::vector vehicles; std::vector objects; }; @@ -110,9 +138,7 @@ struct Sub_Vehicle_t { }; struct SubMessage_t { - // FrameID frame_id{0}; - // std::vector sub_vehicles; }; @@ -141,6 +167,24 @@ inline void to_json(json &j, const Camera_t &o) { {"depthScale", o.depth_scale}, {"outputIndex", o.output_index}}; } +// EventCamera_t +inline void to_json(json &j, const EventCamera_t &o) { + j = json{{"ID", o.ID}, + {"channels", o.channels}, + {"width", o.width}, + {"height", o.height}, + {"fov", o.fov}, + {"T_BC", o.T_BC}, + {"isDepth", o.is_depth}, + {"depthScale", o.depth_scale}, + {"outputIndex", o.output_index}, + {"Cm", o.Cm}, + {"Cp", o.Cp}, + {"sigma_Cp", o.sigma_Cp}, + {"sigma_Cm", o.sigma_Cm}, + {"refractory_period_ns", o.refractory_period_ns}, + {"log_eps", o.log_eps}}; +} // Lidar inline void to_json(json &j, const Lidar_t &o) { @@ -158,6 +202,7 @@ inline void to_json(json &j, const Vehicle_t &o) { {"rotation", o.rotation}, {"size", o.size}, {"cameras", o.cameras}, + {"eventcameras", o.eventcameras}, {"lidars", o.lidars}, {"hasCollisionCheck", o.has_collision_check}}; } @@ -197,6 +242,23 @@ inline void from_json(const json &j, SubMessage_t &o) { o.sub_vehicles = j.at("pub_vehicles").get>(); } +inline void from_json(const json &j, Event_t &o) { + o.coord_x = j.at("coord_x").get(); + o.coord_y = j.at("coord_y").get(); + o.polarity = j.at("polarity").get(); + o.time = j.at("time").get(); +} + +inline void from_json(const json &j, EventsMessage_t &o) { + o.events = j.at("events").get>(); +} + +inline void from_json(const json &j, TimeMessage_t &o) { + o.current_time = j.at("current_time").get(); + o.next_timestep = j.at("next_timestep").get(); + o.rgb_frame = j.at("rgb_frame").get(); +} + inline void to_json(json &j, const PointCloudMessage_t &o) { j = json{{"range", o.range}, {"origin", o.origin}, @@ -205,10 +267,22 @@ inline void to_json(json &j, const PointCloudMessage_t &o) { {"file_name", o.file_name}}; } +inline void to_json(json &j, const Event_t &o) { + j = json{{"coord_x", o.coord_x}, + {"coord_y", o.coord_y}, + {"polarity", o.polarity}, + {"time", o.time}}; +} + +inline void to_json(json &j, const EventsMessage_t &o) { + j = json{{"events", o.events}}; +} + // Struct for outputting parsed received messages to handler functions struct RenderMessage_t { SubMessage_t sub_msg; std::vector images; + // std::vector events; }; } // namespace flightlib \ No newline at end of file diff --git a/flightlib/include/flightlib/common/types.hpp b/flightlib/include/flightlib/common/types.hpp index 69b4e17f9a..7cbb9be7ce 100644 --- a/flightlib/include/flightlib/common/types.hpp +++ b/flightlib/include/flightlib/common/types.hpp @@ -1,6 +1,15 @@ #pragma once #include +#include +#include + +// #include +// #include +// #include +// #include +#define ImageFloatType float + namespace flightlib { @@ -9,6 +18,7 @@ namespace flightlib { // Define the scalar type used. using Scalar = float; // numpy float32 + // Define frame id for unity using FrameID = uint64_t; @@ -54,6 +64,13 @@ using Quaternion = Eigen::Quaternion; template using Ref = Eigen::Ref; +// Using time for events +// using Time = ze::int64_t; +// using uint16_t = ze::uint16_t; +// using uint16_t =std::unit16_t; +// using Time = std::int64_t; + + // // Using `ConstRef` for constant references. // template // using ConstRef = const Eigen::Ref; @@ -65,4 +82,46 @@ using Map = Eigen::Map; static constexpr Scalar Gz = -9.81; const Vector<3> GVEC{0.0, 0.0, Gz}; +// struct Event +// { +// Event(uint16_t x, uint16_t y, Time t, bool pol) +// : x(x), +// y(y), +// t(t), +// pol(pol) +// { + +// } + +// uint16_t x; +// uint16_t y; +// Time t; +// bool pol; +// }; + +struct Event_t { + int coord_x; + int coord_y; + int polarity; + int32_t time; +}; + +struct TimeMessage_t { + int64_t current_time; + int64_t next_timestep; + bool rgb_frame; +}; +using EventsVector = std::vector; + +using Image = cv::Mat_; +using ImagePtr = std::shared_ptr; +using ImagePtrVector = std::vector; + +using RGBImage = cv::Mat; +using RGBImagePtr = std::shared_ptr; +using RGBImagePtrVector = std::vector; + + + + } // namespace flightlib diff --git a/flightlib/include/flightlib/objects/quadrotor.hpp b/flightlib/include/flightlib/objects/quadrotor.hpp index 725e12d302..b04012affe 100644 --- a/flightlib/include/flightlib/objects/quadrotor.hpp +++ b/flightlib/include/flightlib/objects/quadrotor.hpp @@ -8,6 +8,7 @@ #include "flightlib/common/types.hpp" #include "flightlib/dynamics/quadrotor_dynamics.hpp" #include "flightlib/objects/object_base.hpp" +#include "flightlib/sensors/event_camera.hpp" #include "flightlib/sensors/imu.hpp" #include "flightlib/sensors/rgb_camera.hpp" @@ -40,13 +41,17 @@ class Quadrotor : ObjectBase { Vector<3> getPosition(void) const; Quaternion getQuaternion(void) const; std::vector> getCameras(void) const; + std::vector> getEventCameras(void) const; + bool getCamera(const size_t cam_id, std::shared_ptr camera) const; + bool getEventCamera(const size_t cam_id, std::shared_ptr camera) const; // public set functions bool setState(const QuadState& state); bool setCommand(const Command& cmd); bool updateDynamics(const QuadrotorDynamics& dynamics); bool addRGBCamera(std::shared_ptr camera); + bool addEventCamera(std::shared_ptr camera); // low-level controller Vector<4> runFlightCtl(const Scalar sim_dt, const Vector<3>& omega, @@ -70,6 +75,7 @@ class Quadrotor : ObjectBase { IMU imu_; std::unique_ptr integrator_ptr_; std::vector> rgb_cameras_; + std::vector> event_cameras_; // quad control command Command cmd_; diff --git a/flightlib/include/flightlib/sensors/event_camera.hpp b/flightlib/include/flightlib/sensors/event_camera.hpp new file mode 100644 index 0000000000..fce3f581b0 --- /dev/null +++ b/flightlib/include/flightlib/sensors/event_camera.hpp @@ -0,0 +1,113 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "flightlib/common/logger.hpp" +#include "flightlib/common/types.hpp" +#include "flightlib/sensors/sensor_base.hpp" + +namespace flightlib { + +struct Event { + int coord_x; + int coord_y; + int polarity; + float time; +}; + +class EventCamera : SensorBase { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EventCamera(); + ~EventCamera(); + + // public set functions + bool setRelPose(const Ref> B_r_BC, const Ref> R_BC); + bool setWidth(const int width); + bool setHeight(const int height); + bool setFOV(const Scalar fov); + bool setCp(const float cp); + bool setCm(const float cm); + bool setsigmaCp(const float sigma_cp); + bool setsigmaCm(const float sigma_cm); + bool setRefractory(const uint64_t refractory_period); + bool setLogEps(const float log_eps); + bool setImgStore(const bool img_store); + + bool changeTime(TimeMessage_t time_msg); + double getSecSimTime(); + int64_t getMicroSimTime(); + int64_t getMicroTime(); + int64_t getNanoSimTime(); + + bool feedImageQueue(const cv::Mat& image_mat); + bool feedEventImageQueue(const cv::Mat& image_mat); + bool feedEventQueue(std::vector& events); + bool deleteEventQueue(); + + // public get functions + Matrix<4, 4> getRelPose(void) const; + int getChannels(void) const; + int getWidth(void) const; + int getHeight(void) const; + Scalar getFOV(void) const; + float getCm(void) const; + float getCp(void) const; + float getsigmaCm(void) const; + float getsigmaCp(void) const; + uint64_t getRefractory(void) const; + float getLogEps(void) const; + bool getImgStore(void); + + + // Scalar getDepthScale(void) const; + bool getRGBImage(cv::Mat& rgb_img); + bool getEventImages(cv::Mat& image_mat); + std::vector getEvents(); + cv::Mat createEventimages(); + + private: + Logger logger_{"RBGCamera"}; + + // camera parameters + int channels_; + int width_; + int height_; + Scalar fov_; + float cm_; + float cp_; + float sigma_cp_; + float sigma_cm_; + int64_t refractory_period_ns_; + float log_eps_; + + int64_t sim_time = 0; + int64_t delta_time = 0; + int64_t real_time; + + + // Camera relative + Vector<3> B_r_BC_; + Matrix<4, 4> T_BC_; + + // image data buffer + std::mutex queue_mutex_; + const int queue_size_ = 1; + + std::deque rgb_queue_; + std::deque event_image_queue_; + std::deque> event_queue_; + std::vector event_queue_for_img; + std::vector event_queue_for_test; + std::vector event_queue_sum; + bool store_image_ = false; +}; +} // namespace flightlib diff --git a/flightlib/package.xml b/flightlib/package.xml index a837a3a56b..4623d3836c 100644 --- a/flightlib/package.xml +++ b/flightlib/package.xml @@ -10,7 +10,7 @@ catkin catkin_simple - + ze_common gtest \ No newline at end of file diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp index e9a2800cf7..faa8fa67e4 100644 --- a/flightlib/src/bridges/unity_bridge.cpp +++ b/flightlib/src/bridges/unity_bridge.cpp @@ -24,6 +24,7 @@ bool UnityBridge::initializeConnections() { pub_.bind(client_address_ + ":" + pub_port_); // create and bind a download_socket + logger_.info(sub_port_); sub_.set(zmqpp::socket_option::receive_high_water_mark, 6); sub_.bind(client_address_ + ":" + sub_port_); @@ -81,6 +82,7 @@ bool UnityBridge::sendInitialSettings(void) { msg << "Pose"; // create JSON object for initial settings json json_mesg = settings_; + std::cout << settings_.vehicles[0].eventcameras.size(); msg << json_mesg.dump(); // send message without blocking pub_.send(msg, true); @@ -113,12 +115,6 @@ bool UnityBridge::getRender(const FrameID frame_id) { pub_msg_.vehicles[idx].rotation = quaternionRos2Unity(quad_state.q()); } - // for (size_t idx = 0; idx < pub_msg_.objects.size(); idx++) { - // std::shared_ptr> gate = unity_dynamic_gate_[object_i.ID]; - // pub_msg_.objects[idx].position = positionROS2Unity(gate->getPos()); - // pub_msg_.objects[idx].rotation = rotationROS2Unity(gate->getQuat()); - // } - // create new message object zmqpp::message msg; // add topic header @@ -131,12 +127,32 @@ bool UnityBridge::getRender(const FrameID frame_id) { return true; } +bool UnityBridge::trigggerEvents(const FrameID frame_id) { + pub_msg_.frame_id = frame_id; + QuadState quad_state; + for (size_t idx = 0; idx < pub_msg_.vehicles.size(); idx++) { + unity_quadrotors_[idx]->getState(&quad_state); + pub_msg_.vehicles[idx].position = positionRos2Unity(quad_state.p); + pub_msg_.vehicles[idx].rotation = quaternionRos2Unity(quad_state.q()); + } + + // create new message object + zmqpp::message msg; + // add topic header + msg << "Event"; + // create JSON object for pose update and append + json json_msg = pub_msg_; + msg << json_msg.dump(); + // send message without blocking + pub_.send(msg, true); + return true; +} + bool UnityBridge::setScene(const SceneID& scene_id) { if (scene_id >= UnityScene::SceneNum) { logger_.warn("Scene ID is not defined, cannot set scene."); return false; } - // logger_.info("Scene ID is set to %d.", scene_id); settings_.scene_id = scene_id; return true; } @@ -171,13 +187,37 @@ bool UnityBridge::addQuadrotor(std::shared_ptr quad) { camera_t.is_depth = false; camera_t.output_index = cam_idx; vehicle_t.cameras.push_back(camera_t); - // add rgb_cameras rgb_cameras_.push_back(rgb_cameras[cam_idx]); } + + std::vector> event_cameras = + quad->getEventCameras(); + for (size_t cam_idx = 0; cam_idx < event_cameras.size(); cam_idx++) { + std::shared_ptr cam = event_cameras[cam_idx]; + EventCamera_t eventcamera_t; + eventcamera_t.ID = vehicle_t.ID + "_event_" + std::to_string(cam_idx); + eventcamera_t.T_BC = + transformationRos2Unity(event_cameras[cam_idx]->getRelPose()); + eventcamera_t.width = event_cameras[cam_idx]->getWidth(); + eventcamera_t.height = event_cameras[cam_idx]->getHeight(); + eventcamera_t.fov = event_cameras[cam_idx]->getFOV(); + eventcamera_t.is_depth = false; + eventcamera_t.output_index = cam_idx; + eventcamera_t.Cm = event_cameras[cam_idx]->getCm(); + eventcamera_t.Cp = event_cameras[cam_idx]->getCp(); + eventcamera_t.sigma_Cm = event_cameras[cam_idx]->getsigmaCm(); + eventcamera_t.sigma_Cp = event_cameras[cam_idx]->getsigmaCp(); + eventcamera_t.refractory_period_ns = + event_cameras[cam_idx]->getRefractory(); + eventcamera_t.log_eps = event_cameras[cam_idx]->getLogEps(); + vehicle_t.eventcameras.push_back(eventcamera_t); + // add event_cameras + event_cameras_.push_back(event_cameras[cam_idx]); + } + unity_quadrotors_.push_back(quad); - // settings_.vehicles.push_back(vehicle_t); pub_msg_.vehicles.push_back(vehicle_t); return true; @@ -197,12 +237,20 @@ bool UnityBridge::addStaticObject(std::shared_ptr static_object) { } bool UnityBridge::handleOutput() { + bool always = false; + handleOutput(always); +} + +bool UnityBridge::handleOutput(bool always) { // create new message object zmqpp::message msg; + logger_.info(sub_port_); + sub_.receive(msg); + // unpack message metadata std::string json_sub_msg = msg.get(0); - // parse metadata + // // parse metadata SubMessage_t sub_msg = json::parse(json_sub_msg); size_t image_i = 1; @@ -216,61 +264,151 @@ bool UnityBridge::handleOutput() { for (size_t layer_idx = 0; layer_idx <= cam.enabled_layers.size(); layer_idx++) { if (!layer_idx == 0 && !cam.enabled_layers[layer_idx - 1]) continue; - uint32_t image_len = cam.width * cam.height * cam.channels; - // Get raw image bytes from ZMQ message. - // WARNING: This is a zero-copy operation that also casts the input to - // an array of unit8_t. when the message is deleted, this pointer is - // also dereferenced. - const uint8_t* image_data; - msg.get(image_data, image_i); - image_i = image_i + 1; - // Pack image into cv::Mat - cv::Mat new_image = - cv::Mat(cam.height, cam.width, CV_MAKETYPE(CV_8U, cam.channels)); - memcpy(new_image.data, image_data, image_len); - // Flip image since OpenCV origin is upper left, but Unity's is lower - // left. - cv::flip(new_image, new_image, 0); - - // Tell OpenCv that the input is RGB. - if (cam.channels == 3) { - cv::cvtColor(new_image, new_image, CV_RGB2BGR); + if (layer_idx == 1) { + // depth + uint32_t image_len = cam.width * cam.height * 4; + // Get raw image bytes from ZMQ message. + // WARNING: This is a zero-copy operation that also casts the input to + // an array of unit8_t. when the message is deleted, this pointer is + // also dereferenced. + const uint8_t* image_data; + msg.get(image_data, image_i); + image_i = image_i + 1; + // Pack image into cv::Mat + cv::Mat new_image = cv::Mat(cam.height, cam.width, CV_32FC1); + memcpy(new_image.data, image_data, image_len); + // Flip image since OpenCV origin is upper left, but Unity's is lower + // left. + new_image = new_image * (100.f); + cv::flip(new_image, new_image, 0); + + + unity_quadrotors_[idx] + ->getCameras()[cam.output_index] + ->feedImageQueue(layer_idx, new_image); + + + } else { + uint32_t image_len = cam.width * cam.height * cam.channels; + // Get raw image bytes from ZMQ message. + // WARNING: This is a zero-copy operation that also casts the input to + // an array of unit8_t. when the message is deleted, this pointer is + // also dereferenced. + const uint8_t* image_data; + msg.get(image_data, image_i); + image_i = image_i + 1; + // Pack image into cv::Mat + cv::Mat new_image = + cv::Mat(cam.height, cam.width, CV_MAKETYPE(CV_8U, cam.channels)); + memcpy(new_image.data, image_data, image_len); + // Flip image since OpenCV origin is upper left, but Unity's is lower + // left. + cv::flip(new_image, new_image, 0); + + // Tell OpenCv that the input is RGB. + if (cam.channels == 3) { + cv::cvtColor(new_image, new_image, CV_RGB2BGR); + } + unity_quadrotors_[idx] + ->getCameras()[cam.output_index] + ->feedImageQueue(layer_idx, new_image); } - unity_quadrotors_[idx]->getCameras()[cam.output_index]->feedImageQueue( - layer_idx, new_image); } } + // feed events to the eventcamera + for (const auto& cam : settings_.vehicles[idx].eventcameras) { + uint32_t image_len = cam.width * cam.height * cam.channels; + // Get raw image bytes from ZMQ message. + // WARNING: This is a zero-copy operation that also casts the input to + // an array of unit8_t. when the message is deleted, this pointer is + // also dereferenced. + const uint8_t* image_data; + msg.get(image_data, image_i); + image_i = image_i + 1; + // Pack image into cv::Mat + cv::Mat new_image = + cv::Mat(cam.height, cam.width, CV_MAKETYPE(CV_8U, cam.channels)); + memcpy(new_image.data, image_data, image_len); + if (new_image.empty()) { + logger_.warn("the image is empty"); + + return false; + } + // Flip image since OpenCV origin is upper left, but Unity's is lower + // left. + cv::flip(new_image, new_image, 0); + // Tell OpenCv that the input is RGB. + if (cam.channels == 3) { + cv::cvtColor(new_image, new_image, CV_RGB2BGR); + } + // store events + std::string json_msg = msg.get(image_i); + image_i = image_i + 1; + + EventsMessage_t events_ = json::parse(json_msg).get(); + + std::sort(events_.events.begin(), events_.events.end(), + [](const Event_t& a, const Event_t& b) -> bool { + return a.time < b.time; + }); + + unity_quadrotors_[idx] + ->getEventCameras()[cam.output_index] + ->feedEventQueue(events_.events); + + std::string time_msg = msg.get(image_i); + image_i = image_i + 1; + TimeMessage_t timestep = json::parse(time_msg).get(); + // bool always = true; + + unity_quadrotors_[idx]->getEventCameras()[cam.output_index]->setImgStore( + timestep.rgb_frame); + + // if (always || timestep.rgb_frame) + { // timestep.rgb_frame + unity_quadrotors_[idx] + ->getEventCameras()[cam.output_index] + ->feedImageQueue(new_image); + } + std::string amount = std::to_string(timestep.next_timestep); + logger_.info("Next timestep:"); + logger_.info(amount); + unity_quadrotors_[idx]->getEventCameras()[cam.output_index]->changeTime( + timestep); + } } return true; -} +} // namespace flightlib + bool UnityBridge::getPointCloud(PointCloudMessage_t& pointcloud_msg, Scalar time_out) { - // create new message object - zmqpp::message msg; - // add topic header - msg << "PointCloud"; - // create JSON object for initial settings - json json_msg = pointcloud_msg; - msg << json_msg.dump(); - // send message without blocking - pub_.send(msg, true); + // // create new message object + // zmqpp::message msg; + // // add topic header + // msg << "PointCloud"; + // // create JSON object for initial settings + // json json_msg = pointcloud_msg; + // msg << json_msg.dump(); + // // send message without blocking + // pub_.send(msg, true); - std::cout << "Generate PointCloud: Timeout=" << (int)time_out << " seconds." - << std::endl; + // std::cout << "Generate PointCloud: Timeout=" << (int)time_out << " + // seconds." + // << std::endl; - Scalar run_time = 0.0; - while (!std::experimental::filesystem::exists( - pointcloud_msg.path + pointcloud_msg.file_name + ".ply")) { - if (run_time >= time_out) { - logger_.warn("Timeout... PointCloud was not saved within expected time."); - return false; - } - std::cout << "Waiting for Pointcloud: Current Runtime=" << (int)run_time - << " seconds." << std::endl; - usleep((time_out / 10.0) * 1e6); - run_time += time_out / 10.0; - } + // Scalar run_time = 0.0; + // while (!std::experimental::filesystem::exists( + // pointcloud_msg.path + pointcloud_msg.file_name + ".ply")) { + // if (run_time >= time_out) { + // logger_.warn("Timeout... PointCloud was not saved within expected + // time."); return false; + // } + // std::cout << "Waiting for Pointcloud: Current Runtime=" << (int)run_time + // << " seconds." << std::endl; + // usleep((time_out / 10.0) * 1e6); + // run_time += time_out / 10.0; + // } return true; } diff --git a/flightlib/src/objects/quadrotor.cpp b/flightlib/src/objects/quadrotor.cpp index 82c1ed38f5..00d1290968 100644 --- a/flightlib/src/objects/quadrotor.cpp +++ b/flightlib/src/objects/quadrotor.cpp @@ -239,6 +239,11 @@ bool Quadrotor::addRGBCamera(std::shared_ptr camera) { return true; } + bool Quadrotor::addEventCamera(std::shared_ptr camera){ + event_cameras_.push_back(camera); + return true; +} + Vector<3> Quadrotor::getSize(void) const { return size_; } Vector<3> Quadrotor::getPosition(void) const { return state_.p; } @@ -247,6 +252,11 @@ std::vector> Quadrotor::getCameras(void) const { return rgb_cameras_; }; +std::vector> Quadrotor::getEventCameras( + void) const { + return event_cameras_; +}; + bool Quadrotor::getCamera(const size_t cam_id, std::shared_ptr camera) const { if (cam_id <= rgb_cameras_.size()) { @@ -257,4 +267,14 @@ bool Quadrotor::getCamera(const size_t cam_id, return true; } +bool Quadrotor::getEventCamera(const size_t cam_id, + std::shared_ptr camera) const { + if (cam_id <= event_cameras_.size()) { + return false; + } + + camera = event_cameras_[cam_id]; + return true; +} + } // namespace flightlib diff --git a/flightlib/src/sensors/event_camera.cpp b/flightlib/src/sensors/event_camera.cpp new file mode 100644 index 0000000000..7e3027d623 --- /dev/null +++ b/flightlib/src/sensors/event_camera.cpp @@ -0,0 +1,253 @@ +#include "flightlib/sensors/event_camera.hpp" + +namespace flightlib { + +EventCamera::EventCamera() + : channels_(3), width_(720), height_(480), fov_{70.0} { + auto time = std::chrono::high_resolution_clock::now(); +} + +EventCamera::~EventCamera() {} + +bool EventCamera::feedEventImageQueue(const cv::Mat& event) { + queue_mutex_.lock(); + event_image_queue_.push_back(event); + queue_mutex_.unlock(); + return true; +} + +bool EventCamera::feedEventQueue(std::vector& events) { + queue_mutex_.lock(); + event_queue_.push_back(events); + event_queue_for_img.resize(events.size()); + event_queue_for_img = events; + event_queue_for_test = events; + event_queue_sum.insert(std::end(event_queue_sum), std::begin(events), + std::end(events)); + queue_mutex_.unlock(); + std::string amount = std::to_string(event_queue_for_img.size()); + logger_.warn(amount); + return true; +} + +bool EventCamera::setRelPose(const Ref> B_r_BC, + const Ref> R_BC) { + if (!B_r_BC.allFinite() || !R_BC.allFinite()) { + logger_.error( + "The setting value for Camera Relative Pose Matrix is not valid, discard " + "the setting."); + return false; + } + B_r_BC_ = B_r_BC; + T_BC_.block<3, 3>(0, 0) = R_BC; + T_BC_.block<3, 1>(0, 3) = B_r_BC; + T_BC_.row(3) << 0.0, 0.0, 0.0, 1.0; + return true; +} + +bool EventCamera::setWidth(const int width) { + if (width <= 0.0) { + logger_.warn( + "The setting value for Image Width is not valid, discard the setting."); + return false; + } + width_ = width; + return true; +} + +bool EventCamera::setHeight(const int height) { + if (height <= 0.0) { + logger_.warn( + "The setting value for Image Height is not valid, discard the " + "setting."); + return false; + } + height_ = height; + return true; +} + +bool EventCamera::setFOV(const Scalar fov) { + if (fov <= 0.0) { + logger_.warn( + "The setting value for Camera Field-of-View is not valid, discard the " + "setting."); + return false; + } + fov_ = fov; + return true; +} +bool EventCamera::setCp(const float cp) { + if (cp <= 0.0) { + logger_.warn( + "The setting value for Eventcamera Cp is not valid, discard the " + "setting."); + return false; + } + cp_ = cp; + return true; +} +bool EventCamera::setCm(const float cm) { + if (cm <= 0.0) { + logger_.warn( + "The setting value for Eventcamera Cm is not valid, discard the " + "setting."); + return false; + } + cm_ = cm; + return true; +} +bool EventCamera::setsigmaCp(const float sigma_cp) { + if (sigma_cp <= 0.0) { + logger_.warn( + "The setting value for Eventcamera sigmaCm is not valid, discard the " + "setting."); + return false; + } + sigma_cp_ = sigma_cp; + return true; +} +bool EventCamera::setsigmaCm(const float sigma_cm) { + if (sigma_cm <= 0.0) { + logger_.warn( + "The setting value for Eventcamera sigmaCm is not valid, discard the " + "setting."); + return false; + } + sigma_cm_ = sigma_cm; + return true; +} +bool EventCamera::setRefractory(const uint64_t refractory_period) { + if (refractory_period <= 0) { + logger_.warn( + "The setting value for Eventcamera refractory period is not valid, " + "discard the " + "setting."); + return false; + } + refractory_period_ns_ = refractory_period; + return true; +} +bool EventCamera::setLogEps(const float log_eps) { + if (log_eps <= 0.0) { + logger_.warn( + "The setting value for Eventcamera log_eps is not valid, discard the " + "setting."); + return false; + } + log_eps_ = log_eps; + return true; +} +bool EventCamera::setImgStore(const bool img_store) { + store_image_ = img_store; + return true; +} + +bool EventCamera::changeTime(TimeMessage_t time_msg) { + if (time_msg.next_timestep <= 0) { + logger_.warn("timestep is zero or invalid"); + return false; + } + sim_time = time_msg.current_time; + delta_time = time_msg.next_timestep; + return true; +} +double EventCamera::getSecSimTime() { return sim_time / 1000000.0; } +int64_t EventCamera::getMicroSimTime() { return sim_time; } +int64_t EventCamera::getNanoSimTime() { return sim_time * 1000; } +int64_t EventCamera::getMicroTime() { return real_time; } + +Matrix<4, 4> EventCamera::getRelPose(void) const { return T_BC_; } + +int EventCamera::getWidth(void) const { return width_; } + +int EventCamera::getHeight(void) const { return height_; } + +Scalar EventCamera::getFOV(void) const { return fov_; } +float EventCamera::getCm(void) const { return cm_; } +float EventCamera::getCp(void) const { return cp_; } + +float EventCamera::getsigmaCm(void) const { return sigma_cm_; } +float EventCamera::getsigmaCp(void) const { return sigma_cp_; } +uint64_t EventCamera::getRefractory(void) const { + return refractory_period_ns_; +} +float EventCamera::getLogEps(void) const { return log_eps_; } +bool EventCamera::getImgStore(void) { return store_image_; } + +bool EventCamera::getEventImages(cv::Mat& event) { + if (!event_image_queue_.empty()) { + // seems wrong here + event = event_image_queue_.front(); + event_image_queue_.pop_front(); + return true; + } + return false; +} + +std::vector EventCamera::getEvents() { + std::vector events; + if (!event_queue_for_test.empty()) { + // seems wrong here + events = event_queue_for_test; + // event_queue_for_test.clear(); + return events; + } + logger_.error("empty events buffer"); + return events; +} + +bool EventCamera::deleteEventQueue() { + if (!event_queue_for_test.empty()) { + event_queue_for_test.clear(); + return true; + } + logger_.error("already empty event buffer"); + return false; +} + +cv::Mat EventCamera::createEventimages() { + int wid = getWidth(); + int hei = getHeight(); + cv::Mat image = cv::Mat::zeros(cv::Size(wid, hei), CV_64FC1); + std::vector events; + int count = 0; + + for (auto event : event_queue_sum) { + if (event.coord_x > wid || event.coord_y > hei) { + logger_.error("coord out of the image"); + } + if (event.polarity == 1) { + image.at(event.coord_y, event.coord_x)[0] = 0; + image.at(event.coord_y, event.coord_x)[1] = 0; + image.at(event.coord_y, event.coord_x)[2] = 255; + } else if (event.polarity == -1) { + count++; + image.at(event.coord_y, event.coord_x)[0] = 255; + image.at(event.coord_y, event.coord_x)[1] = 0; + image.at(event.coord_y, event.coord_x)[2] = 0; + } + } + event_queue_sum.clear(); + return image; +} + +bool EventCamera::feedImageQueue(const cv::Mat& image_mat) { + queue_mutex_.lock(); + + if (rgb_queue_.size() > queue_size_) rgb_queue_.resize(queue_size_); + rgb_queue_.push_back(image_mat); + + queue_mutex_.unlock(); + return true; +} + +bool EventCamera::getRGBImage(cv::Mat& rgb_img) { + if (!rgb_queue_.empty()) { + rgb_img = rgb_queue_.front(); + rgb_queue_.pop_front(); + return true; + } + return false; +} + +} // namespace flightlib \ No newline at end of file diff --git a/flightros/CMakeLists.txt b/flightros/CMakeLists.txt index c51fd8ce21..dd00a44d26 100644 --- a/flightros/CMakeLists.txt +++ b/flightros/CMakeLists.txt @@ -8,6 +8,11 @@ find_package(OpenCV REQUIRED) catkin_simple() +catkin_package( + LIBRARIES + CATKIN_DEPENDS + ) + # Setup Default Build Type as Release if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) @@ -27,29 +32,47 @@ endif () # Setup General C++ Flags SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DEIGEN_STACK_ALLOCATION_LIMIT=1048576") +add_compile_options(-O3) # Setup Release and Debug flags set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_ARCH_FLAGS} -Wall -DNDEBUG -fPIC") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -Wall -g") -cs_add_library(flight_pilot - src/flight_pilot.cpp +include_directories(include) + +cs_add_library(ros_utils + src/ros_utils.cpp ) -target_link_libraries(flight_pilot - ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} - stdc++fs +cs_add_library(rosbag_writer +src/rosbag_writer.cpp ) -cs_add_executable(flight_pilot_node - src/flight_pilot_node.cpp +cs_add_executable(testing +src/test/testing.cpp) + +target_link_libraries(testing + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + stdc++fs + rosbag_writer + ros_utils + zmq + zmqpp + ) -target_link_libraries(flight_pilot_node - flight_pilot - ${OpenCV_LIBRARIES} - stdc++fs +cs_add_executable(record +src/record.cpp) + +target_link_libraries(record + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + stdc++fs + rosbag_writer + ros_utils + zmq + zmqpp ) # Finish diff --git a/flightros/dependencies.yaml b/flightros/dependencies.yaml index 963c3efaa9..71e211cdae 100644 --- a/flightros/dependencies.yaml +++ b/flightros/dependencies.yaml @@ -26,4 +26,32 @@ repositories: rpg_quadrotor_control: type: git url: git@github.com:uzh-rpg/rpg_quadrotor_control.git + version: master + glog_catkin: + type: git + url: git@github.com:ethz-asl/glog_catkin.git + version: master + ze_oss: + type: git + url: git@github.com:uzh-rpg/ze_oss.git + version: master + gflags_catkin: + type: git + url: git@github.com:ethz-asl/gflags_catkin.git + version: master + eigen_checks: + type: git + url: git@github.com:ethz-asl/eigen_checks.git + version: master + minkindr: + type: git + url: git@github.com:ethz-asl/minkindr.git + version: master + minkindr_ros: + type: git + url: git@github.com:ethz-asl/minkindr_ros.git + version: master + yaml_cpp_catkin: + type: git + url: git@github.com:ethz-asl/yaml_cpp_catkin.git version: master \ No newline at end of file diff --git a/flightros/include/flightros/flight_pilot.hpp b/flightros/include/flightros/flight_pilot.hpp deleted file mode 100644 index 0663415e12..0000000000 --- a/flightros/include/flightros/flight_pilot.hpp +++ /dev/null @@ -1,70 +0,0 @@ - -#pragma once - -#include - -// ros -#include -#include - -// rpg quadrotor -#include -#include -#include -#include - -// flightlib -#include "flightlib/bridges/unity_bridge.hpp" -#include "flightlib/common/quad_state.hpp" -#include "flightlib/common/types.hpp" -#include "flightlib/objects/quadrotor.hpp" -#include "flightlib/sensors/rgb_camera.hpp" - -using namespace flightlib; - -namespace flightros { - -class FlightPilot { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - FlightPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); - ~FlightPilot(); - - // callbacks - void mainLoopCallback(const ros::TimerEvent& event); - void poseCallback(const nav_msgs::Odometry::ConstPtr& msg); - - bool setUnity(const bool render); - bool connectUnity(void); - bool loadParams(void); - - private: - // ros nodes - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - // publisher - - // subscriber - ros::Subscriber sub_state_est_; - - // main loop timer - ros::Timer timer_main_loop_; - - // unity quadrotor - std::shared_ptr quad_ptr_; - std::shared_ptr rgb_camera_; - QuadState quad_state_; - - // Flightmare(Unity3D) - std::shared_ptr unity_bridge_ptr_; - SceneID scene_id_{UnityScene::WAREHOUSE}; - bool unity_ready_{false}; - bool unity_render_{false}; - RenderMessage_t unity_output_; - uint16_t receive_id_{0}; - - // auxiliary variables - Scalar main_loop_freq_{50.0}; -}; -} // namespace flightros \ No newline at end of file diff --git a/flightros/include/flightros/record.hpp b/flightros/include/flightros/record.hpp new file mode 100644 index 0000000000..79ee235750 --- /dev/null +++ b/flightros/include/flightros/record.hpp @@ -0,0 +1,90 @@ +#pragma once + +// ros +#include +#include +#include +#include +#include +#include +#include "flightros/ros_utils.hpp" +#include "flightros/rosbag_writer.hpp" +#include "opencv2/imgcodecs.hpp" + + +// standard libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// flightlib +#include "flightlib/bridges/unity_bridge.hpp" +#include "flightlib/bridges/unity_message_types.hpp" +#include "flightlib/common/quad_state.hpp" +#include "flightlib/common/types.hpp" +#include "flightlib/objects/quadrotor.hpp" +#include "flightlib/objects/static_gate.hpp" +#include "flightlib/sensors/event_camera.hpp" +#include "flightlib/sensors/rgb_camera.hpp" + +// trajectory +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "trajectory_generation_helper/polynomial_trajectory_helper.h" + +using namespace flightlib; +using namespace flightros; + +namespace record { + +bool setUnity(const bool render); +bool connectUnity(void); +std::string type2str(int type); +void saveToFile(std::vector); +void createMinSnap(const std::vector waypoints, + quadrotor_common::Trajectory* trajectory); +polynomial_trajectories::PolynomialTrajectory createOwnSnap( + const std::vector waypoints_in, + Eigen::VectorXd segment_times_in); +void samplePolynomial( + quadrotor_common::Trajectory& trajectory, + const polynomial_trajectories::PolynomialTrajectory& polynomial, + const double sampling_frequency); +image_transport::Publisher rgb_pub_; + +// unity quadrotor +std::shared_ptr quad_ptr_; +std::shared_ptr rgb_camera_; +std::shared_ptr event_camera_; +QuadState quad_state_; + +// Flightmare(Unity3D) +std::shared_ptr unity_bridge_ptr_; +SceneID scene_id_{UnityScene::WAREHOUSE}; +bool unity_ready_{false}; +bool unity_render_{true}; +RenderMessage_t unity_output_; +uint16_t receive_id_{0}; +std::ofstream events_text_file_; +int count_; +std::vector errors; +int num_cam = 1; +std::string path_to_output_bag = "/home/gian/bags_flightmare/record.bag"; + +std::shared_ptr writer_; +} // namespace record \ No newline at end of file diff --git a/flightros/include/flightros/ros_utils.hpp b/flightros/include/flightros/ros_utils.hpp new file mode 100644 index 0000000000..878e89becf --- /dev/null +++ b/flightros/include/flightros/ros_utils.hpp @@ -0,0 +1,40 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace flightlib; + +namespace flightros { +inline std::string getTopicName(int i, const std::string& suffix) +{ + std::stringstream ss; + ss << "cam" << i << "/" << suffix; + return ss.str(); +} + +inline std::string getTopicName(const std::string& prefix, int i, const std::string& suffix) +{ + std::stringstream ss; + ss << prefix << "/" << getTopicName(i, suffix); + return ss.str(); +} + +inline ros::Time toRosTime(int64_t t) +{ + ros::Time ros_time; + ros_time.fromNSec(t); + return ros_time; +} + +void imageToMsg(const cv::Mat_& image, int64_t t, sensor_msgs::ImagePtr& msg); +void imageToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg); +void imageFloatToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg); +void eventsToMsg(const EventsVector& events, int width, int height, dvs_msgs::EventArrayPtr& msg, int64_t starting_time); + +} // namespace flightros diff --git a/flightros/include/flightros/rosbag_writer.hpp b/flightros/include/flightros/rosbag_writer.hpp new file mode 100644 index 0000000000..37a13e29d0 --- /dev/null +++ b/flightros/include/flightros/rosbag_writer.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace flightros { + +class RosbagWriter { + public: + RosbagWriter(const std::string& path_to_output_bag); + RosbagWriter(const std::string& path_to_output_bag, int64_t stime); + + ~RosbagWriter(); + + void imageCallback(const ImagePtr& images, int64_t t); + void imageRGBCallback(const RGBImagePtr& images, int64_t t); + void imageOFCallback(const RGBImagePtr& image, int64_t t); + void imageDepthCallback(const RGBImagePtr& image, int64_t t); + void eventsCallback(const EventsVector& events, int64_t t); + void poseCallback(const ze::Transformation& T_W_C, int64_t t); + void imageEventCallback(const RGBImagePtr& images, int64_t t); + + private: + size_t num_cameras_; + cv::Size sensor_size_; + rosbag::Bag bag_; + int64_t starting_time; + const std::string topic_name_prefix_ = ""; + + int64_t last_published_camera_info_time_; + int64_t last_published_image_time_; +}; + +} // namespace flightros diff --git a/flightros/include/flightros/testing.hpp b/flightros/include/flightros/testing.hpp new file mode 100644 index 0000000000..dd3eb37be8 --- /dev/null +++ b/flightros/include/flightros/testing.hpp @@ -0,0 +1,94 @@ +#pragma once + +// ros +#include +#include +#include +#include +#include +#include +#include "opencv2/imgcodecs.hpp" +#include "flightros/rosbag_writer.hpp" +#include "flightros/ros_utils.hpp" + + +// standard libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// flightlib +#include "flightlib/bridges/unity_bridge.hpp" +#include "flightlib/bridges/unity_message_types.hpp" +#include "flightlib/common/quad_state.hpp" +#include "flightlib/common/types.hpp" +#include "flightlib/objects/quadrotor.hpp" +#include "flightlib/objects/static_gate.hpp" +#include "flightlib/sensors/event_camera.hpp" +#include "flightlib/sensors/rgb_camera.hpp" + +// trajectory +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace flightlib; +using namespace flightros; + +namespace testing { + +bool setUnity(const bool render); +bool connectUnity(void); +std::string type2str(int type); +void saveToFile(std::vector); + +// publisher +image_transport::Publisher rgb_pub_; +image_transport::Publisher diff_pub_; +image_transport::Publisher rgb_rgb_pub_; +image_transport::Publisher event_pub_; +image_transport::Publisher of_pub_; +image_transport::Publisher depth_pub_; + +cv::Mat rgb_image; + +// unity quadrotor +std::shared_ptr quad_ptr_; +std::shared_ptr rgb_camera_; +std::shared_ptr event_camera_; +QuadState quad_state_; + +// Flightmare(Unity3D) +std::shared_ptr unity_bridge_ptr_; +SceneID scene_id_{UnityScene::WAREHOUSE}; +bool unity_ready_{false}; +bool unity_render_{true}; +RenderMessage_t unity_output_; +uint16_t receive_id_{0}; +std::ofstream events_text_file_; +int count_; +std::vector values, amount; +std::vector errors; +int num_cam=1; +const std::string path_to_output_bag= "/home/gian/bags_flightmare/out.bag"; +std::shared_ptr writer_; +} // namespace testing diff --git a/flightros/launch/record.launch b/flightros/launch/record.launch new file mode 100644 index 0000000000..5922f1b569 --- /dev/null +++ b/flightros/launch/record.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/flightros/launch/rotors_gazebo.launch b/flightros/launch/rotors_gazebo.launch deleted file mode 100644 index 621e619fec..0000000000 --- a/flightros/launch/rotors_gazebo.launch +++ /dev/null @@ -1,122 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/flightros/launch/rotors_gazebo_test.launch b/flightros/launch/rotors_gazebo_test.launch deleted file mode 100644 index 037d7877d8..0000000000 --- a/flightros/launch/rotors_gazebo_test.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/flightros/launch/testing.launch b/flightros/launch/testing.launch new file mode 100644 index 0000000000..00da127a38 --- /dev/null +++ b/flightros/launch/testing.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/flightros/package.xml b/flightros/package.xml index 63ced44735..a4987509d4 100644 --- a/flightros/package.xml +++ b/flightros/package.xml @@ -16,5 +16,21 @@ autopilot quadrotor_common nav_msgs - + eigen_catkin + quadrotor_msgs + polynomial_trajectories + std_msgs + geometry_msgs + tf + tf_conversions + minkindr_conversions + eigen_conversions + image_transport + cv_bridge + ze_common + ze_matplotlib + ze_cameras + rosbag + dvs_msgs + \ No newline at end of file diff --git a/flightros/params/snaga.yaml b/flightros/params/snaga.yaml new file mode 100644 index 0000000000..368aa478eb --- /dev/null +++ b/flightros/params/snaga.yaml @@ -0,0 +1,326 @@ +# this file contains all trajectories and position or orientations +# additional to this file there should be a local.yaml file that is not +# tracked by git and which contains user specific information +record: + single_position: + - 1.0 + - 40.0 + - 1.0 + single_orientation: + - 0.0 + - 0.0 + - 0.0 + - 0.1 + + japan_one: + x: + - 0.0 + - 0.0 + - -2.0 + - -8.0 + - -14.0 + - -16.0 + - -14.0 + - -2.0 + - 0.0 + - 0.0 + # - 1.0 + # - -1.0 + # - -1.0 + # - -1.0 + + y: + - -24.0 + - -15.0 + - -12.0 + - -11.0 + - -10.0 + - -13.0 + - -14.0 + - -13.0 + - -1.0 + - 13.0 + # - 16.0 + # - -8.0 + # - 0.0 + # - 2.0 + z: + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + + japan_two: + x: + - 1.0 + - -1.0 + - -5.0 + - -8.0 + - -10.0 + - -8.0 + - 0.0 + - 1.0 + - 0.0 + - -1.0 + - -1.0 + - -1.0 + - -1.0 + + y: + - 0.0 + - -11.0 + - -12.0 + - -13.0 + - -12.0 + - -11.0 + - -14.0 + - -16.0 + - -18.0 + - -16.0 + - -8.0 + - 0.0 + - 2.0 + z: + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + + japan_three: + x: + - 1.0 + - 0.0 + - -2.0 + - -1.0 + - 2.0 + - 1.0 + - -1.0 + - 0.0 + - 1.0 + - 0.0 + # - -1.0 + # - -1.0 + # - -1.0 + + y: + - -50.0 + - -45.0 + - -40.0 + - -35.0 + - -30.0 + - -25.0 + - -20.0 + - -15.0 + - -10.0 + - -5.0 + # - -8.0 + # - 0.0 + # - 2.0 + z: + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + + japan_four: + x: + - -7.0 + - -3.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - -1.0 + - -2.0 + - 1.0 + # - 0.0 + # - -1.0 + # - -1.0 + # - -1.0 + + y: + - 89.0 + - 91.0 + - 89.0 + - 86.0 + - 80.0 + - 73.0 + - 69.0 + - 63.0 + - 50.0 + # - -5.0 + # - -8.0 + # - 0.0 + # - 2.0 + z: + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + + japan_five: + x: + - 0.0 + - 3.0 + - 6.0 + - 12.0 + - 16.0 + - 27.0 + - 42.0 + - 50.0 + - 42.0 + - 27.0 + - 16.0 + - 8.0 + - 0.0 + - -18.0 + # - -1.0 + # - -1.0 + # - -1.0 + + y: + - -8.0 + - -11.0 + - -12.0 + - -13.0 + - -13.0 + - -12.8 + - -12.6 + - -12.5 + - -12.4 + - -12.2 + - -12.0 + - -11.0 + - -14.0 + - -11.0 + # - -8.0 + # - 0.0 + # - 2.0 + z: + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + + japan_six: + x: + - -1.0 + - -0.5 + - 0.0 + - 0.5 + - 0.0 + - -1.0 + - 2.0 + - 0.0 + # - 42.0 + # - 27.0 + # - 16.0 + # - 8.0 + # - 0.0 + # - -18.0 + # - -1.0 + # - -1.0 + # - -1.0 + + y: + - 60.0 + - 62.0 + - 64.0 + - 62.0 + - 55.0 + - 48.0 + - 35.0 + - 23.0 + # - -12.4 + # - -12.2 + # - -12.0 + # - -11.0 + # - -14.0 + # - -11.0 + # - -8.0 + # - 0.0 + # - 2.0 + z: + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + # - 2.5 + + + japan_seven: + x: + - 0.0 + - 1.0 + - 2.0 + - 1.0 + - 0.0 + + y: + - 40.0 + - 30.0 + - 20.0 + - 10.0 + - 0.0 + + z: + - 2.5 + - 2.5 + - 2.5 + - 2.5 + - 2.5 diff --git a/flightros/src/flight_pilot.cpp b/flightros/src/flight_pilot.cpp deleted file mode 100644 index b69028ba3c..0000000000 --- a/flightros/src/flight_pilot.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#include "flightros/flight_pilot.hpp" - -namespace flightros { - -FlightPilot::FlightPilot(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) - : nh_(nh), - pnh_(pnh), - scene_id_(UnityScene::WAREHOUSE), - unity_ready_(false), - unity_render_(false), - receive_id_(0), - main_loop_freq_(50.0) { - // load parameters - if (!loadParams()) { - ROS_WARN("[%s] Could not load all parameters.", - pnh_.getNamespace().c_str()); - } else { - ROS_INFO("[%s] Loaded all parameters.", pnh_.getNamespace().c_str()); - } - - // quad initialization - quad_ptr_ = std::make_shared(); - - // add mono camera - rgb_camera_ = std::make_shared(); - Vector<3> B_r_BC(0.0, 0.0, 0.3); - Matrix<3, 3> R_BC = Quaternion(1.0, 0.0, 0.0, 0.0).toRotationMatrix(); - std::cout << R_BC << std::endl; - rgb_camera_->setFOV(90); - rgb_camera_->setWidth(720); - rgb_camera_->setHeight(480); - rgb_camera_->setRelPose(B_r_BC, R_BC); - quad_ptr_->addRGBCamera(rgb_camera_); - - // initialization - quad_state_.setZero(); - quad_ptr_->reset(quad_state_); - - - // initialize subscriber call backs - sub_state_est_ = nh_.subscribe("flight_pilot/state_estimate", 1, - &FlightPilot::poseCallback, this); - - timer_main_loop_ = nh_.createTimer(ros::Rate(main_loop_freq_), - &FlightPilot::mainLoopCallback, this); - - - // wait until the gazebo and unity are loaded - ros::Duration(5.0).sleep(); - - // connect unity - setUnity(unity_render_); - connectUnity(); -} - -FlightPilot::~FlightPilot() {} - -void FlightPilot::poseCallback(const nav_msgs::Odometry::ConstPtr &msg) { - quad_state_.x[QS::POSX] = (Scalar)msg->pose.pose.position.x; - quad_state_.x[QS::POSY] = (Scalar)msg->pose.pose.position.y; - quad_state_.x[QS::POSZ] = (Scalar)msg->pose.pose.position.z; - quad_state_.x[QS::ATTW] = (Scalar)msg->pose.pose.orientation.w; - quad_state_.x[QS::ATTX] = (Scalar)msg->pose.pose.orientation.x; - quad_state_.x[QS::ATTY] = (Scalar)msg->pose.pose.orientation.y; - quad_state_.x[QS::ATTZ] = (Scalar)msg->pose.pose.orientation.z; - // - quad_ptr_->setState(quad_state_); - - if (unity_render_ && unity_ready_) { - unity_bridge_ptr_->getRender(0); - unity_bridge_ptr_->handleOutput(); - } -} - -void FlightPilot::mainLoopCallback(const ros::TimerEvent &event) { - // empty -} - -bool FlightPilot::setUnity(const bool render) { - unity_render_ = render; - if (unity_render_ && unity_bridge_ptr_ == nullptr) { - // create unity bridge - unity_bridge_ptr_ = UnityBridge::getInstance(); - unity_bridge_ptr_->addQuadrotor(quad_ptr_); - ROS_INFO("[%s] Unity Bridge is created.", pnh_.getNamespace().c_str()); - } - return true; -} - -bool FlightPilot::connectUnity() { - if (!unity_render_ || unity_bridge_ptr_ == nullptr) return false; - unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_); - return unity_ready_; -} - -bool FlightPilot::loadParams(void) { - // load parameters - quadrotor_common::getParam("main_loop_freq", main_loop_freq_, pnh_); - quadrotor_common::getParam("unity_render", unity_render_, pnh_); - - return true; -} - -} // namespace flightros \ No newline at end of file diff --git a/flightros/src/flight_pilot_node.cpp b/flightros/src/flight_pilot_node.cpp deleted file mode 100644 index a405c838f5..0000000000 --- a/flightros/src/flight_pilot_node.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include - -#include "flightros/flight_pilot.hpp" - -int main(int argc, char** argv) { - ros::init(argc, argv, "flight_pilot"); - flightros::FlightPilot pilot(ros::NodeHandle(), ros::NodeHandle("~")); - - // spin the ros - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/flightros/src/record.cpp b/flightros/src/record.cpp new file mode 100644 index 0000000000..6180f48408 --- /dev/null +++ b/flightros/src/record.cpp @@ -0,0 +1,478 @@ +#include "flightros/record.hpp" + +bool record::setUnity(const bool render) { + unity_render_ = render; + if (unity_render_ && unity_bridge_ptr_ == nullptr) { + // create unity bridge + unity_bridge_ptr_ = UnityBridge::getInstance(); + unity_bridge_ptr_->addQuadrotor(quad_ptr_); + std::cout << "Unity Bridge is created." << std::endl; + } + return true; +} + +bool record::connectUnity() { + if (!unity_render_ || unity_bridge_ptr_ == nullptr) return false; + unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_); + return unity_ready_; +} + +void record::samplePolynomial( + quadrotor_common::Trajectory& trajectory, + const polynomial_trajectories::PolynomialTrajectory& polynomial, + const double sampling_frequency) { + if (polynomial.trajectory_type == + polynomial_trajectories::TrajectoryType::UNDEFINED) { + ROS_WARN_STREAM("Trajectory is undefined."); + } + + trajectory.points.push_back(polynomial.start_state); + + const ros::Duration dt(1.0 / sampling_frequency); + ros::Duration time_from_start = polynomial.start_state.time_from_start + dt; + ROS_INFO_STREAM("Sampling." << polynomial.T << "/" << dt); + ROS_INFO_STREAM("From start." << time_from_start); + + while (time_from_start < polynomial.T) { + trajectory.points.push_back(polynomial_trajectories::getPointFromTrajectory( + polynomial, time_from_start)); + time_from_start += dt; + } + ROS_INFO_STREAM("From start to end." << time_from_start); + + trajectory.points.push_back(polynomial.end_state); + + trajectory.trajectory_type = + quadrotor_common::Trajectory::TrajectoryType::GENERAL; +} + +std::string record::type2str(int type) { + std::string r; + + uchar depth = type & CV_MAT_DEPTH_MASK; + uchar chans = 1 + (type >> CV_CN_SHIFT); + + switch (depth) { + case CV_8U: + r = "8U"; + break; + case CV_8S: + r = "8S"; + break; + case CV_16U: + r = "16U"; + break; + case CV_16S: + r = "16S"; + break; + case CV_32S: + r = "32S"; + break; + case CV_32F: + r = "32F"; + break; + case CV_64F: + r = "64F"; + break; + default: + r = "User"; + break; + } + + r += "C"; + r += (chans + '0'); + + return r; +} + +void record::createMinSnap(const std::vector waypoints, + quadrotor_common::Trajectory* trajectory) { + // identify the segment times + Eigen::VectorXd segment_times = Eigen::VectorXd::Ones(waypoints.size() - 1); + double desired_speed = 1.0; + for (int i = 0; i < waypoints.size() - 1; i++) { + segment_times[i] = + (waypoints.at(i + 1) - waypoints.at(i)).norm() / desired_speed; + ROS_INFO_STREAM("seg time" << segment_times[i]); + } + Eigen::VectorXd minimization_weights(4); + minimization_weights << 0.0, 0.0, 0.0, 100.0; + double sampling_freq = 50.0; + quadrotor_common::TrajectoryPoint start_state; + start_state.position = waypoints.front(); + start_state.velocity = + Eigen::Vector3d::UnitX() * + 0.05; // hack to get a smooth heading trajectory at the beginning + quadrotor_common::TrajectoryPoint end_state; + end_state.position = waypoints.back(); + std::vector waypoints_no_start_end = waypoints; + waypoints_no_start_end.erase(waypoints_no_start_end.begin()); + waypoints_no_start_end.pop_back(); + std::cout << "generating trajectory through " << waypoints_no_start_end.size() + << " waypoints with " << segment_times.size() << " segment times" + << std::endl; + polynomial_trajectories::PolynomialTrajectorySettings trajectory_settings; + trajectory_settings.way_points = waypoints_no_start_end; + trajectory_settings.minimization_weights = minimization_weights; + trajectory_settings.polynomial_order = 11; + trajectory_settings.continuity_order = 4; + double max_velocity = 5.0; + double max_collective_thrust = 20.0; + double max_roll_pitch_rate = 3.0; + + + *trajectory = + trajectory_generation_helper::polynomials::computeTimeOptimalTrajectory( + start_state, end_state, 3, max_velocity, max_collective_thrust, + max_roll_pitch_rate, sampling_freq); +} + +polynomial_trajectories::PolynomialTrajectory record::createOwnSnap( + const std::vector waypoints_in, + Eigen::VectorXd segment_times_in) { + double desired_speed_in = 1.0; + for (int i = 0; i < waypoints_in.size() - 1; i++) { + segment_times_in[i] = + (waypoints_in.at(i + 1) - waypoints_in.at(i)).norm() / desired_speed_in; + ROS_INFO_STREAM("seg time: " << segment_times_in[i]); + } + + Eigen::VectorXd minimization_weights_in(4); + minimization_weights_in << 0.0, 0.0, 0.0, 100.0; + double sampling_freq_in = 50.0; + quadrotor_common::TrajectoryPoint start_state_in; + start_state_in.position = waypoints_in.front(); + start_state_in.velocity = + Eigen::Vector3d::UnitX() * + 0.05; // hack to get a smooth heading trajectory at the beginning + quadrotor_common::TrajectoryPoint end_state_in; + end_state_in.position = waypoints_in.back(); + // maybe the problem lies in the time from start + std::vector waypoints_no_start_end_in = waypoints_in; + waypoints_no_start_end_in.erase(waypoints_no_start_end_in.begin()); + waypoints_no_start_end_in.pop_back(); + std::cout << "generating trajectory through " + << waypoints_no_start_end_in.size() << " waypoints with " + << segment_times_in.size() << " segment times" << std::endl; + polynomial_trajectories::PolynomialTrajectorySettings trajectory_settings_in; + trajectory_settings_in.way_points = waypoints_no_start_end_in; + trajectory_settings_in.minimization_weights = minimization_weights_in; + trajectory_settings_in.polynomial_order = 18; + trajectory_settings_in.continuity_order = 4; + double max_velocity_in = 5.0; + double max_collective_thrust_in = 20.0; + double max_roll_pitch_rate_in = 3.0; + + polynomial_trajectories::PolynomialTrajectory trajectory_in = + polynomial_trajectories::minimum_snap_trajectories:: + generateMinimumSnapTrajectory( + segment_times_in, start_state_in, end_state_in, trajectory_settings_in, + max_velocity_in, max_collective_thrust_in, max_roll_pitch_rate_in); + + return trajectory_in; +} + +void record::saveToFile(std::vector events) { + for (const Event_t& e : events) { + if (e.polarity != 0) { + record::events_text_file_ << e.time << " " << e.coord_x << " " + << e.coord_y << " " << e.polarity << std::endl; + } + } +} + +int main(int argc, char* argv[]) { + // initialize ROS + ros::init(argc, argv, "flightmare_gates"); + ros::NodeHandle nh(""); + ros::NodeHandle pnh("~"); + image_transport::ImageTransport my_image_transport(nh); + + ros::Rate(50.0); + + // Settings + bool rotate = false; + bool trajectory_rotation = false; + bool position_trajectory; + bool ring_traj = false; + int scene; + double cp; + double cm; + float time_for_rotation; + std::string which_trajectory; + // pull nitialization param from param server + pnh.getParam("/rosbag/rotate", rotate); + pnh.getParam("/rosbag/trajectory_rotation", trajectory_rotation); + pnh.getParam("/rosbag/position_trajectory", position_trajectory); + pnh.getParam("/rosbag/path", record::path_to_output_bag); + pnh.getParam("/rosbag/ring_traj", ring_traj); + pnh.getParam("/rosbag/scene_id", scene); + pnh.getParam("/rosbag/trajectory", which_trajectory); + pnh.getParam("/rosbag/cp", cp); + pnh.getParam("/rosbag/cm", cm); + pnh.getParam("/rosbag/time_for_rotation", time_for_rotation); + + + record::scene_id_ = scene; + + // quad initialization + record::quad_ptr_ = std::make_unique(); + + // add camera + record::rgb_camera_ = std::make_unique(); + record::event_camera_ = std::make_unique(); + + record::rgb_pub_ = my_image_transport.advertise("camera/rgb", 1); + ros::Publisher trajectory_pub_ = + nh.advertise("autopilot/trajectory", 1); + ros::Publisher traj_pub_ = nh.advertise("path/trajectory", 1); + ros::Publisher line_pub_ = nh.advertise("path/straight", 1); + + + std::vector position_vec; + std::vector orientation_vec; + std::vector waypts_x; + std::vector waypts_y; + std::vector waypts_z; + + pnh.getParam("/record/single_position", position_vec); + pnh.getParam("/record/single_orientation", orientation_vec); + pnh.getParam(which_trajectory + "/x", waypts_x); + pnh.getParam(which_trajectory + "/y", waypts_y); + pnh.getParam(which_trajectory + "/z", waypts_z); + + ROS_INFO_STREAM("writing to rosbag: " << record::path_to_output_bag); + + + int frame = 0; + // Flightmare + Vector<3> B_r_BC(0.0, 0.0, 0.3); + Matrix<3, 3> R_BC = Quaternion(1.0, 0.0, 0.0, 0.0).toRotationMatrix(); + record::rgb_camera_->setFOV(83); + record::rgb_camera_->setWidth(512); + record::rgb_camera_->setHeight(352); + record::rgb_camera_->setRelPose(B_r_BC, R_BC); + record::rgb_camera_->setPostProcesscing( + std::vector{true, false, true}); // depth, segmentation, optical flow + record::quad_ptr_->addRGBCamera(record::rgb_camera_); + record::event_camera_->setFOV(83); + record::event_camera_->setWidth(512); + record::event_camera_->setHeight(352); + record::event_camera_->setRelPose(B_r_BC, R_BC); + record::event_camera_->setCp(0.1); + record::event_camera_->setCm(0.1); + record::event_camera_->setsigmaCm(cp); + record::event_camera_->setsigmaCp(cm); + record::event_camera_->setRefractory(1); + record::event_camera_->setLogEps(0.0001); + + record::quad_ptr_->addEventCamera(record::event_camera_); + + + // double cp = record::event_camera_->getCp(); + // double cm = record::event_camera_->getCm(); + bool record = false; + // // initialization + record::quad_state_.setZero(); + + record::quad_ptr_->reset(record::quad_state_); + // record::quad_ptr_->setState() + + record::writer_ = std::make_shared(record::path_to_output_bag); + + // Set unity bridge + record::setUnity(record::unity_render_); + + // connect unity + record::connectUnity(); + std::vector way_points; + + Eigen::VectorXd seg_times(waypts_x.size() - 1); + + quadrotor_common::Trajectory sampled_trajectory_; + + // create trajectory ring or not + if (ring_traj) { + ROS_WARN_STREAM("Creating RING trajectory"); + way_points.push_back(Eigen::Vector3d(0, 10, 2.5)); + way_points.push_back(Eigen::Vector3d(5, 0, 2.5)); + way_points.push_back(Eigen::Vector3d(0, -10, 2.5)); + way_points.push_back(Eigen::Vector3d(-5, 0, 2.5)); + way_points.push_back(Eigen::Vector3d(-7, 10, 2.5)); + way_points.push_back(Eigen::Vector3d(-5, 20, 2.5)); + + std::size_t num_waypoints = way_points.size(); + Eigen::VectorXd segment_times(num_waypoints); + segment_times << 50.0, 50.0, 50.0, 50.0, 50.0, 50.0; + Eigen::VectorXd minimization_weights(num_waypoints); + minimization_weights << 0.0, 0.0, 0.0, 1.0; + + polynomial_trajectories::PolynomialTrajectorySettings trajectory_settings = + polynomial_trajectories::PolynomialTrajectorySettings( + way_points, minimization_weights, 7, 4); + + polynomial_trajectories::PolynomialTrajectory trajectory = + polynomial_trajectories::minimum_snap_trajectories:: + generateMinimumSnapRingTrajectory(segment_times, trajectory_settings, + 20.0, 20.0, 6.0); + } else { + for (int it = 0; it < waypts_x.size(); it++) { + way_points.push_back( + Eigen::Vector3d(waypts_x.at(it), waypts_y.at(it), waypts_z.at(it))); + } + for (int it = 0; it < waypts_x.size() - 1; it++) { + seg_times[it] = 10.0; + } + } + polynomial_trajectories::PolynomialTrajectory trajectory_ = + record::createOwnSnap(way_points, seg_times); + + record::samplePolynomial(sampled_trajectory_, trajectory_, 10.0); + + nav_msgs::Path path; + path.header.frame_id = "/map"; + for (auto pt : sampled_trajectory_.points) { + geometry_msgs::PoseStamped pose; + pose.header.frame_id = "/map"; + pose.pose.position.x = pt.position[0]; + pose.pose.position.y = pt.position[1]; + pose.pose.position.z = pt.position[2]; + path.poses.push_back(pose); + ROS_INFO_STREAM("pt " << pt.position[0] << "/" << pt.position[1] << "/" + << pt.position[2]); + } + + // Desired path in lines + nav_msgs::Path desired_path; + desired_path.header.frame_id = "/map"; + + for (auto pt : way_points) { + geometry_msgs::PoseStamped pose; + pose.header.frame_id = "/map"; + pose.pose.position.x = pt[0]; + pose.pose.position.y = pt[1]; + pose.pose.position.z = pt[2]; + desired_path.poses.push_back(pose); + } + + cv::Mat new_image, of_image, depth_image, ev_image; + Image I; + + ROS_INFO_STREAM( + "========================================================================" + "="); + + while (ros::ok() && record::unity_render_ && record::unity_ready_) { + quadrotor_common::TrajectoryPoint desired_pose = + polynomial_trajectories::getPointFromTrajectory( + trajectory_, ros::Duration(record::event_camera_->getSecSimTime())); + + // different options are considered: + // pure rotation, pure trnaslation or both + if (position_trajectory) { + record::quad_state_.x[QS::POSX] = (Scalar)desired_pose.position.x(); + record::quad_state_.x[QS::POSY] = (Scalar)desired_pose.position.y(); + record::quad_state_.x[QS::POSZ] = (Scalar)desired_pose.position.z(); + + } else { + record::quad_state_.x[QS::POSX] = position_vec.at(0); + record::quad_state_.x[QS::POSY] = position_vec.at(1); + record::quad_state_.x[QS::POSZ] = position_vec.at(2); + } + if (trajectory_rotation) { + record::quad_state_.x[QS::ATTW] = (Scalar)desired_pose.orientation.w(); + record::quad_state_.x[QS::ATTX] = (Scalar)desired_pose.orientation.x(); + record::quad_state_.x[QS::ATTY] = (Scalar)desired_pose.orientation.y(); + record::quad_state_.x[QS::ATTZ] = (Scalar)desired_pose.orientation.z(); + } else if (rotate) { + record::quad_state_.x[QS::ATTW] = + 2 * std::cos(record::event_camera_->getSecSimTime() * 0.1) * + std::cos(record::event_camera_->getSecSimTime() * 3.1415 / + time_for_rotation); + // should be changing velocity and directions + record::quad_state_.x[QS::ATTX] = 0.0; + record::quad_state_.x[QS::ATTY] = 0.0; + record::quad_state_.x[QS::ATTZ] = + 2 * std::cos(record::event_camera_->getSecSimTime() * 0.1) * + std::sin(record::event_camera_->getSecSimTime() * 3.1425 / + time_for_rotation); + } else { + record::quad_state_.x[QS::ATTW] = orientation_vec.at(3); + record::quad_state_.x[QS::ATTX] = orientation_vec.at(0); + record::quad_state_.x[QS::ATTY] = orientation_vec.at(1); + record::quad_state_.x[QS::ATTZ] = orientation_vec.at(2); + } + + // publish trajectory for visualization + traj_pub_.publish(path); + line_pub_.publish(desired_path); + + // define new position and orientation + ze::Transformation twc; + record::quad_state_.qx.normalize(); + twc.getRotation() = ze::Quaternion( + record::quad_state_.x[QS::ATTW], record::quad_state_.x[QS::ATTX], + record::quad_state_.x[QS::ATTY], record::quad_state_.x[QS::ATTZ]); + twc.getPosition() = ze::Position(record::quad_state_.x[QS::POSX], + record::quad_state_.x[QS::POSY], + record::quad_state_.x[QS::POSZ]); + // writ position to rosbag + record::writer_->poseCallback(twc, record::event_camera_->getNanoSimTime()); + + ROS_INFO_STREAM("pose " << record::quad_state_.x[QS::POSX] << "/" + << record::quad_state_.x[QS::POSY] << "/" + << record::quad_state_.x[QS::POSZ] << "/" + << record::event_camera_->getSecSimTime()); + + // send state to server + record::quad_ptr_->setState(record::quad_state_); + record::unity_bridge_ptr_->getRender(0); + record::unity_bridge_ptr_->handleOutput(); + + // get images + record::event_camera_->getRGBImage(ev_image); + record::rgb_camera_->getRGBImage(new_image); + record::rgb_camera_->getOpticalFlow(of_image); + record::rgb_camera_->getDepthMap(depth_image); + + sensor_msgs::ImagePtr rgb_msg = + cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_image).toImageMsg(); + record::rgb_pub_.publish(rgb_msg); + + RGBImagePtr rgb_img_ptr = std::make_shared(ev_image); + RGBImagePtr of_img_ptr = std::make_shared(of_image); + RGBImagePtr depth_img_ptr = std::make_shared(depth_image); + + // triggers for starting recording + if (frame > 10 && record::event_camera_->getImgStore()) { + ROS_INFO_STREAM("starting to record"); + record = true; + } + + // write to rosbag + if (record) { + if (record::event_camera_->getImgStore()) { + cv::Mat event_img = record::event_camera_->createEventimages(); + RGBImagePtr event_img_ptr = std::make_shared(event_img); + + record::writer_->imageRGBCallback( + rgb_img_ptr, record::event_camera_->getNanoSimTime()); + record::writer_->imageOFCallback( + of_img_ptr, record::event_camera_->getNanoSimTime()); + record::writer_->imageDepthCallback( + depth_img_ptr, record::event_camera_->getNanoSimTime()); + record::writer_->imageEventCallback( + event_img_ptr, record::event_camera_->getNanoSimTime()); + } + + const EventsVector& events = record::event_camera_->getEvents(); + record::writer_->eventsCallback(events, + record::event_camera_->getNanoSimTime()); + } + // clear the buffer + record::event_camera_->deleteEventQueue(); + frame++; + } + record::events_text_file_.close(); + return 0; +} diff --git a/flightros/src/ros_utils.cpp b/flightros/src/ros_utils.cpp new file mode 100644 index 0000000000..00f31c52be --- /dev/null +++ b/flightros/src/ros_utils.cpp @@ -0,0 +1,50 @@ +#include +#include + +namespace flightros { + +void imageToMsg(const cv::Mat_& image, int64_t t, + sensor_msgs::ImagePtr& msg) { + cv_bridge::CvImage cv_image; + cv_image.image = image; + cv_image.encoding = "bgr8"; + cv_image.header.stamp = toRosTime(t); + msg = cv_image.toImageMsg(); +} +void imageToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg) { + cv_bridge::CvImage cv_image; + cv_image.image = image; + cv_image.encoding = "bgr8"; + cv_image.header.stamp = toRosTime(t); + msg = cv_image.toImageMsg(); +} +void imageFloatToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg) { + cv_bridge::CvImage cv_image; + cv_image.image = image; + cv_image.encoding = "32FC1"; + cv_image.header.stamp = toRosTime(t); + msg = cv_image.toImageMsg(); +} + +void eventsToMsg(const EventsVector& events, int width, int height, + dvs_msgs::EventArrayPtr& msg, int64_t starting_time) { + std::vector events_list; + for (const Event_t& e : events) { + dvs_msgs::Event ev; + ev.x = e.coord_x; + ev.y = e.coord_y; + int64_t event_time=e.time; + ev.ts = toRosTime((event_time*1000 + starting_time) ); + ev.polarity = e.polarity; + if(e.time>0){ + events_list.push_back(ev); + } + } + + msg->events = events_list; + msg->height = height; + msg->width = width; + msg->header.stamp = events_list.back().ts; +} + +} // namespace flightros diff --git a/flightros/src/rosbag_writer.cpp b/flightros/src/rosbag_writer.cpp new file mode 100644 index 0000000000..71b4818154 --- /dev/null +++ b/flightros/src/rosbag_writer.cpp @@ -0,0 +1,145 @@ +#include + +DECLARE_double(ros_publisher_camera_info_rate); +DECLARE_double(ros_publisher_frame_rate); +DECLARE_double(ros_publisher_depth_rate); +DECLARE_double(ros_publisher_pointcloud_rate); +DECLARE_double(ros_publisher_optic_flow_rate); +DEFINE_string(path_to_output_bag, "", + "Path to which save the output bag file."); + +namespace flightros { +RosbagWriter::RosbagWriter(const std::string& path_to_output_bag) { + num_cameras_ = 1; + sensor_size_ = cv::Size(100, 100); + starting_time = 1; + try { + bag_.open(path_to_output_bag, rosbag::bagmode::Write); + } catch (rosbag::BagIOException e) { + LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag + << std::endl; + return; + } + + last_published_camera_info_time_ = 0; + last_published_image_time_ = 0; +} +RosbagWriter::RosbagWriter(const std::string& path_to_output_bag, + int64_t stime) { + num_cameras_ = 1; + sensor_size_ = cv::Size(100, 100); + + starting_time = 1; // stime + try { + bag_.open(path_to_output_bag, rosbag::bagmode::Write); + } catch (rosbag::BagIOException e) { + LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag + << std::endl; + return; + } + last_published_camera_info_time_ = 0; + last_published_image_time_ = 0; +} + +RosbagWriter::~RosbagWriter() { + LOG(INFO) << "Finalizing the bag..."; + bag_.close(); + LOG(INFO) << "Finished writing to bag: " << FLAGS_path_to_output_bag; +} + +void RosbagWriter::imageCallback(const ImagePtr& image, int64_t t) { + sensor_size_ = image->size(); + if (image) { + sensor_msgs::ImagePtr msg; + imageToMsg(*image, t + starting_time, msg); + bag_.write(getTopicName(topic_name_prefix_, 0, "image_raw"), + msg->header.stamp, msg); + } + last_published_image_time_ = t + starting_time; +} +void RosbagWriter::imageRGBCallback(const RGBImagePtr& image, int64_t t) { + sensor_size_ = image->size(); + ROS_INFO_STREAM("writing" << image); + if (image) { + sensor_msgs::ImagePtr msg; + imageToMsg(*image, t + starting_time, msg); + bag_.write(getTopicName(topic_name_prefix_, 0, "image_rgb_raw"), + msg->header.stamp, msg); + } + last_published_image_time_ = t + starting_time; +} +void RosbagWriter::imageEventCallback(const RGBImagePtr& image, int64_t t) { + sensor_size_ = image->size(); + if (image) { + sensor_msgs::ImagePtr msg; + imageToMsg(*image, t + starting_time, msg); + bag_.write(getTopicName(topic_name_prefix_, 0, "image_event"), + msg->header.stamp, msg); + } + last_published_image_time_ = t + starting_time; +} + +void RosbagWriter::imageOFCallback(const RGBImagePtr& image, int64_t t) { + sensor_size_ = image->size(); + if (image) { + sensor_msgs::ImagePtr msg; + imageToMsg(*image, t + starting_time, msg); + bag_.write(getTopicName(topic_name_prefix_, 0, "image_of"), + msg->header.stamp, msg); + } + last_published_image_time_ = t + starting_time; +} +void RosbagWriter::imageDepthCallback(const RGBImagePtr& image, int64_t t) { + sensor_size_ = image->size(); + if (image) { + sensor_msgs::ImagePtr msg; + imageFloatToMsg(*image, t + starting_time, msg); + bag_.write(getTopicName(topic_name_prefix_, 0, "image_depth"), + msg->header.stamp, msg); + } + + last_published_image_time_ = t + starting_time; +} + + +void RosbagWriter::eventsCallback(const EventsVector& events, int64_t t) { + if (sensor_size_.width == 0 || sensor_size_.height == 0) { + ROS_WARN_STREAM("width to small"); + return; + } + + if (events.empty()) { + ROS_WARN_STREAM("empty "); + return; + } + + dvs_msgs::EventArrayPtr msg; + msg.reset(new dvs_msgs::EventArray); + eventsToMsg(events, sensor_size_.width, sensor_size_.height, msg, + starting_time); + + bag_.write(getTopicName(topic_name_prefix_, 0, "events"), msg->header.stamp, + msg); +} + +void RosbagWriter::poseCallback(const ze::Transformation& T_W_C, int64_t t) { + geometry_msgs::PoseStamped pose_stamped_msg; + geometry_msgs::TransformStamped transform_stamped_msg; + transform_stamped_msg.header.frame_id = "map"; + transform_stamped_msg.header.stamp = toRosTime(t + starting_time); + tf::tfMessage tf_msg; + + tf::poseStampedKindrToMsg(T_W_C, toRosTime(t + starting_time), "map", + &pose_stamped_msg); + bag_.write(getTopicName(topic_name_prefix_, 0, "pose"), + toRosTime(t + starting_time), pose_stamped_msg); + + // Write tf transform to bag + std::stringstream ss; + ss << "cam"; + transform_stamped_msg.child_frame_id = ss.str(); + tf::transformKindrToMsg(T_W_C, &transform_stamped_msg.transform); + tf_msg.transforms.push_back(transform_stamped_msg); + bag_.write("/tf", toRosTime(t + starting_time), tf_msg); +} +} // namespace flightros \ No newline at end of file diff --git a/flightros/src/test/testing.cpp b/flightros/src/test/testing.cpp new file mode 100644 index 0000000000..0131fb4032 --- /dev/null +++ b/flightros/src/test/testing.cpp @@ -0,0 +1,392 @@ +#include "flightros/testing.hpp" + +bool testing::setUnity(const bool render) { + unity_render_ = render; + if (unity_render_ && unity_bridge_ptr_ == nullptr) { + // create unity bridge + unity_bridge_ptr_ = UnityBridge::getInstance(); + unity_bridge_ptr_->addQuadrotor(quad_ptr_); + std::cout << "Unity Bridge is created." << std::endl; + } + return true; +} + +bool testing::connectUnity() { + if (!unity_render_ || unity_bridge_ptr_ == nullptr) return false; + unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_); + return unity_ready_; +} + +// return image type +std::string testing::type2str(int type) { + std::string r; + + uchar depth = type & CV_MAT_DEPTH_MASK; + uchar chans = 1 + (type >> CV_CN_SHIFT); + + switch (depth) { + case CV_8U: + r = "8U"; + break; + case CV_8S: + r = "8S"; + break; + case CV_16U: + r = "16U"; + break; + case CV_16S: + r = "16S"; + break; + case CV_32S: + r = "32S"; + break; + case CV_32F: + r = "32F"; + break; + case CV_64F: + r = "64F"; + break; + default: + r = "User"; + break; + } + + r += "C"; + r += (chans + '0'); + + return r; +} +// saves the events into a .txt file +void testing::saveToFile(std::vector events) { + for (const Event_t& e : events) { + if (e.polarity != 0) { + testing::events_text_file_ << e.time << " " << e.coord_x << " " + << e.coord_y << " " << e.polarity << std::endl; + } + } +} + +int main(int argc, char* argv[]) { + // initialize ROS + ros::init(argc, argv, "flightmare_gates"); + ros::NodeHandle nh(""); + ros::NodeHandle pnh("~"); + ros::Rate(50.0); + image_transport::ImageTransport my_image_transport(nh); + std::vector times, mean_errors, stddev_errors; + + // quad initialization + testing::quad_ptr_ = std::make_unique(); + // add camera + testing::rgb_camera_ = std::make_unique(); + testing::event_camera_ = std::make_unique(); + + testing::scene_id_ = 4; + // Flightmare + Vector<3> B_r_BC(0.0, 0.0, 0.3); + Matrix<3, 3> R_BC = Quaternion(1.0, 0.0, 0.0, 0.0).toRotationMatrix(); + // initialize rgb camera + testing::rgb_camera_->setFOV(83); + testing::rgb_camera_->setWidth(512); + testing::rgb_camera_->setHeight(352); + testing::rgb_camera_->setRelPose(B_r_BC, R_BC); + testing::rgb_camera_->setPostProcesscing( + std::vector{true, false, true}); // depth, segmentation, optical flow + testing::quad_ptr_->addRGBCamera(testing::rgb_camera_); + + // initialize event camera, careful with the image size. + // h,w should be a mutliple of 8 otherwise the thread groups in the compute + // shader need to be adapted + testing::event_camera_->setFOV(83); + testing::event_camera_->setWidth(512); + testing::event_camera_->setHeight(352); + testing::event_camera_->setRelPose(B_r_BC, R_BC); + testing::event_camera_->setCp(0.2); + testing::event_camera_->setCm(0.2); + testing::event_camera_->setsigmaCm(0.0); + testing::event_camera_->setsigmaCp(0.0); + testing::event_camera_->setRefractory(1); + testing::event_camera_->setLogEps(0.0001); + testing::quad_ptr_->addEventCamera(testing::event_camera_); + + // scene can be changed + // testing::scene_id_ = 1; + double cp = testing::event_camera_->getCp(); + double cm = testing::event_camera_->getCm(); + + // // initialization + testing::quad_state_.setZero(); + testing::quad_ptr_->reset(testing::quad_state_); + + // ROS publishers for control + testing::rgb_pub_ = + my_image_transport.advertise("camera/antialiasing_rgb", 1); + testing::rgb_rgb_pub_ = my_image_transport.advertise("camera/rgb", 1); + testing::diff_pub_ = my_image_transport.advertise("camera/diff", 1); + testing::event_pub_ = my_image_transport.advertise("camera/event", 1); + testing::of_pub_ = my_image_transport.advertise("camera/of", 1); + testing::depth_pub_ = my_image_transport.advertise("camera/depth", 1); + + testing::writer_ = std::make_shared( + testing::path_to_output_bag, testing::event_camera_->getMicroSimTime()); + + // Set unity bridge + testing::setUnity(testing::unity_render_); + + // connect unity + testing::connectUnity(); + + // Define path + std::vector way_points; + way_points.push_back(Eigen::Vector3d(0, 10, 2.5)); + way_points.push_back(Eigen::Vector3d(5, 0, 2.5)); + way_points.push_back(Eigen::Vector3d(0, -10, 2.5)); + way_points.push_back(Eigen::Vector3d(-5, 0, 2.5)); + + std::size_t num_waypoints = way_points.size(); + Eigen::VectorXd segment_times(num_waypoints); + segment_times << 10.0, 10.0, 10.0, 10.0; + Eigen::VectorXd minimization_weights(num_waypoints); + minimization_weights << 1.0, 1.0, 1.0, 1.0; + + polynomial_trajectories::PolynomialTrajectorySettings trajectory_settings = + polynomial_trajectories::PolynomialTrajectorySettings( + way_points, minimization_weights, 7, 4); + + polynomial_trajectories::PolynomialTrajectory trajectory = + polynomial_trajectories::minimum_snap_trajectories:: + generateMinimumSnapRingTrajectory(segment_times, trajectory_settings, + 20.0, 20.0, 6.0); + + testing::events_text_file_.open("/home/gian/Desktop/events"); + + // Start testing + bool is_first_image = true; + Image I, L, L_reconstructed, L_last; + + int frame = 0; + float time_ = 0; + while (ros::ok() && testing::unity_render_ && testing::unity_ready_ && + frame < 200) { + + quadrotor_common::TrajectoryPoint desired_pose = + polynomial_trajectories::getPointFromTrajectory(trajectory, + ros::Duration(time_)); + ROS_INFO_STREAM("pose time " << time_); + testing::quad_state_.x[QS::POSX] = (Scalar)desired_pose.position.x(); + testing::quad_state_.x[QS::POSY] = (Scalar)desired_pose.position.y(); + testing::quad_state_.x[QS::POSZ] = (Scalar)desired_pose.position.z(); + testing::quad_state_.x[QS::ATTW] = (Scalar)desired_pose.orientation.w(); + testing::quad_state_.x[QS::ATTX] = (Scalar)desired_pose.orientation.x(); + testing::quad_state_.x[QS::ATTY] = (Scalar)desired_pose.orientation.y(); + testing::quad_state_.x[QS::ATTZ] = (Scalar)desired_pose.orientation.z(); + + ze::Transformation tcv; + testing::quad_state_.qx.normalize(); + tcv.getRotation() = ze::Quaternion( + testing::quad_state_.x[QS::ATTW], testing::quad_state_.x[QS::ATTX], + testing::quad_state_.x[QS::ATTY], testing::quad_state_.x[QS::ATTZ]); + tcv.getPosition() = ze::Position((Scalar)desired_pose.position.x(), + (Scalar)desired_pose.position.y(), + (Scalar)desired_pose.position.z()); + testing::quad_ptr_->setState(testing::quad_state_); + ROS_INFO_STREAM("time " << testing::event_camera_->getSecSimTime()); + + testing::unity_bridge_ptr_->getRender(0); + testing::unity_bridge_ptr_->handleOutput(true); + + //make some checks + cv::Mat new_image, rgb_img, of_img, depth_img; + testing::event_camera_->getRGBImage(new_image); + + // publish rgb image + sensor_msgs::ImagePtr rgb_msg = + cv_bridge::CvImage(std_msgs::Header(), "bgr8", new_image).toImageMsg(); + testing::rgb_pub_.publish(rgb_msg); + + + // publish rgb image without antialiasing + testing::rgb_camera_->getRGBImage(rgb_img); + sensor_msgs::ImagePtr rrgb_msg = + cv_bridge::CvImage(std_msgs::Header(), "bgr8", rgb_img).toImageMsg(); + testing::rgb_rgb_pub_.publish(rrgb_msg); + + // publish event image + cv::Mat ev_img = testing::event_camera_->createEventimages(); + sensor_msgs::ImagePtr ev_msg = + cv_bridge::CvImage(std_msgs::Header(), "bgr8", ev_img).toImageMsg(); + testing::event_pub_.publish(ev_msg); + + // publish OF image + bool was_of_empty = testing::rgb_camera_->getOpticalFlow(of_img); + sensor_msgs::ImagePtr of_msg = + cv_bridge::CvImage(std_msgs::Header(), "bgr8", of_img).toImageMsg(); + testing::of_pub_.publish(of_msg); + + // publish depth image + bool was_depth_empty = testing::rgb_camera_->getDepthMap(depth_img); + sensor_msgs::ImagePtr depth_msg = + cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg(); + testing::depth_pub_.publish(depth_msg); + + ROS_INFO_STREAM("recieved OF and depth " << was_of_empty << " / "<< was_depth_empty); + ROS_INFO_STREAM("new image type " << testing::type2str(of_img.type())); + + // coonvert rgb image to gray + cv::cvtColor(new_image, new_image, CV_BGR2GRAY); + new_image.convertTo(I, cv::DataType::type, 1. / 255.); + + // calculate the events to test + Image dummie = + cv::Mat::zeros(I.rows, I.cols, cv::DataType::type); + + // convert image to log image + Image log_img; + cv::log(I + 0.0001, log_img); + + cv::log(dummie + 0.0001, dummie); + L = log_img - dummie; + + + if (is_first_image) { + // Initialize reconstructed image from the ground truth image + L_reconstructed = L.clone(); + L_last = L.clone(); + is_first_image = false; + } + + int counter = 0; + int counter_ = 0; + int count = 0; + bool first_check = true; + + // create illuminance difference image from events + Image event_image = + cv::Mat::zeros(I.rows, I.cols, cv::DataType::type); + + for (const Event_t& e : testing::event_camera_->getEvents()) { + counter_++; + if (e.polarity != 0) { + int pol; + if (e.polarity == 1) { + pol = 1; + count++; + } else if (e.polarity == -1) { + pol = -1; + counter++; + } else + pol = 0; + + L_reconstructed(e.coord_y, e.coord_x) += pol * cp; + event_image(e.coord_y, e.coord_x) += pol * cp; + + if (first_check) { + ROS_INFO_STREAM("Time of first event: " << e.time); + first_check = false; + } + } + } + + ROS_INFO_STREAM("Amount of pos events " << count << " of neg " << counter + << " of " << counter_); + + const EventsVector& events = testing::event_camera_->getEvents(); + testing::saveToFile(testing::event_camera_->getEvents()); + // clear the buffer + testing::event_camera_->deleteEventQueue(); + + // starting evaluations + // total error + ImageFloatType total_error = 0; + for (int y = 0; y < I.rows; ++y) { + for (int x = 0; x < I.cols; ++x) { + const ImageFloatType reconstruction_error = + std::fabs(L_reconstructed(y, x) - L(y, x)); + total_error += reconstruction_error; + } + } + ROS_INFO_STREAM("Total error " << total_error); + + // calculate total difference of two consecutive images + total_error = 0; + float true_mean = 0; + float true_error_mean = 0; + int count_for_mean = 0; + for (int y = 0; y < I.rows; ++y) { + for (int x = 0; x < I.cols; ++x) { + total_error += std::fabs(L_last(y, x) - L(y, x)); + if (std::fabs(L_last(y, x) - L(y, x)) != 0) { + true_mean += std::fabs(L_last(y, x) - L(y, x)); + true_error_mean += std::fabs(L_reconstructed(y, x) - L(y, x)); + count_for_mean++; + } + } + } + float number = static_cast(count_for_mean); + true_mean = true_mean / (number); + true_error_mean = true_error_mean / (number); + ROS_INFO_STREAM("Total diference between two images " << total_error); + + ROS_INFO_STREAM("True means " << true_mean + << ", error mean : " << true_error_mean); + + // compute std deviation and mean of the error + Image error, real_diff; + cv::absdiff(L_reconstructed, L, error); + cv::absdiff(L, L_last, real_diff); + + + double minVal, maxVal, minVal_, maxVal_; + cv::Point max, min, max_, min_; + cv::minMaxLoc(error, &minVal, &maxVal, &min, &max); + cv::minMaxLoc(real_diff, &minVal_, &maxVal_, &min_, &max_); + + // publish the two images + cv::Mat draw; + error.convertTo(draw, CV_8U, 255.0 / (maxVal - minVal), -minVal); + sensor_msgs::ImagePtr diff_msg = + cv_bridge::CvImage(std_msgs::Header(), "mono8", draw).toImageMsg(); + testing::diff_pub_.publish(diff_msg); + + + cv::Scalar mean_error, stddev_error, mean, stddevv; + cv::meanStdDev(real_diff, mean, stddevv); + cv::meanStdDev(error, mean_error, stddev_error); + ROS_INFO_STREAM("Mean of real diff: " << mean << ", Stddev: " << stddevv); + ROS_INFO_STREAM("Mean error: " << mean_error + << ", Stddev: " << stddev_error); + cv::Mat draw_; + real_diff.convertTo(draw_, CV_8U, 255.0 / (maxVal_ - minVal_), -minVal_); + ROS_INFO_STREAM("The biggest val of last " << maxVal_ << " and " << minVal_ + << " at " << max_.y << "/" + << max_.x); + ROS_INFO_STREAM("The biggest val of reconstructed " + << maxVal << " and " << minVal << " at " << max.y << "/" + << max.x); + + // setup variables for next rendering + if (frame < 10 || frame % 1 == 0) L_reconstructed = L.clone(); + else if (frame > 10) { + times.push_back(frame); + mean_errors.push_back(mean_error[0]); + stddev_errors.push_back(stddev_error[0]); + } + L_last = L.clone(); + + time_ += 0.001; + frame++; + } + // Plot the mean and stddev reconstruction error over time + ze::plt::plot(times, mean_errors, "r"); + ze::plt::plot(times, stddev_errors, "b--"); + std::stringstream title; + title << "C = " << cp << ", sigma = " << testing::event_camera_->getsigmaCm() + << ", bias = " << 0; + ze::plt::title(title.str()); + ze::plt::xlabel("Time (s)"); + ze::plt::ylabel("Mean / Stddev reconstruction error"); + ze::plt::grid(true); + ze::plt::show(); + testing::events_text_file_.close(); + return 0; +} From b9e2d97db23ea0b01bda90e15f0aecf7056fff8b Mon Sep 17 00:00:00 2001 From: gerni17 Date: Sat, 27 Mar 2021 23:14:02 +0100 Subject: [PATCH 2/7] added changes of comments --- flightros/src/racing/racing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/flightros/src/racing/racing.cpp b/flightros/src/racing/racing.cpp index a412be3e03..d5d09489ef 100644 --- a/flightros/src/racing/racing.cpp +++ b/flightros/src/racing/racing.cpp @@ -32,6 +32,7 @@ int main(int argc, char *argv[]) { // Flightmare(Unity3D) std::shared_ptr unity_bridge_ptr = UnityBridge::getInstance(); SceneID scene_id{UnityScene::WAREHOUSE}; + scene_id = 4; bool unity_ready{false}; // unity quadrotor From 196c437022ad23a9cfe318cad9b2c42f6538a6b5 Mon Sep 17 00:00:00 2001 From: gerni17 Date: Sat, 27 Mar 2021 23:46:52 +0100 Subject: [PATCH 3/7] fixed corner case of zero events --- flightros/src/ros_utils.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/flightros/src/ros_utils.cpp b/flightros/src/ros_utils.cpp index 00f31c52be..24e5111933 100644 --- a/flightros/src/ros_utils.cpp +++ b/flightros/src/ros_utils.cpp @@ -1,6 +1,7 @@ -#include #include +#include + namespace flightros { void imageToMsg(const cv::Mat_& image, int64_t t, @@ -18,7 +19,8 @@ void imageToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg) { cv_image.header.stamp = toRosTime(t); msg = cv_image.toImageMsg(); } -void imageFloatToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg) { +void imageFloatToMsg(const cv::Mat& image, int64_t t, + sensor_msgs::ImagePtr& msg) { cv_bridge::CvImage cv_image; cv_image.image = image; cv_image.encoding = "32FC1"; @@ -33,18 +35,21 @@ void eventsToMsg(const EventsVector& events, int width, int height, dvs_msgs::Event ev; ev.x = e.coord_x; ev.y = e.coord_y; - int64_t event_time=e.time; - ev.ts = toRosTime((event_time*1000 + starting_time) ); + int64_t event_time = e.time; + ev.ts = toRosTime((event_time * 1000 + starting_time)); ev.polarity = e.polarity; - if(e.time>0){ - events_list.push_back(ev); + if (e.time > 0) { + events_list.push_back(ev); } } msg->events = events_list; msg->height = height; msg->width = width; - msg->header.stamp = events_list.back().ts; + if (events_list.size() == 0) { + msg->header.stamp = toRosTime(starting_time + 1); + } else { + msg->header.stamp = events_list.back().ts; + } } - } // namespace flightros From 3063be31d81d8c11ee5922ec4e2b2e09acb4cb9a Mon Sep 17 00:00:00 2001 From: gerni17 Date: Mon, 5 Apr 2021 14:11:59 +0200 Subject: [PATCH 4/7] most changes for yulongs comments --- flightros/CMakeLists.txt | 6 +++--- flightros/params/snaga.yaml | 2 +- flightros/src/{ => event_camera}/record.cpp | 0 flightros/src/{ => utils}/ros_utils.cpp | 0 flightros/src/{ => utils}/rosbag_writer.cpp | 0 5 files changed, 4 insertions(+), 4 deletions(-) rename flightros/src/{ => event_camera}/record.cpp (100%) rename flightros/src/{ => utils}/ros_utils.cpp (100%) rename flightros/src/{ => utils}/rosbag_writer.cpp (100%) diff --git a/flightros/CMakeLists.txt b/flightros/CMakeLists.txt index 29523a4420..b0a881e691 100644 --- a/flightros/CMakeLists.txt +++ b/flightros/CMakeLists.txt @@ -44,7 +44,7 @@ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -Wall -g") include_directories(include) cs_add_library(ros_utils - src/ros_utils.cpp) + src/utils/ros_utils.cpp) #pilot cs_add_library(flight_pilot @@ -68,7 +68,7 @@ target_link_libraries(flight_pilot_node ) cs_add_library(rosbag_writer -src/rosbag_writer.cpp +src/utils/rosbag_writer.cpp ) cs_add_executable(testing @@ -86,7 +86,7 @@ target_link_libraries(testing cs_add_executable(record -src/record.cpp) +src/event_camera/record.cpp) target_link_libraries(record ${catkin_LIBRARIES} diff --git a/flightros/params/snaga.yaml b/flightros/params/snaga.yaml index 368aa478eb..6f9990323f 100644 --- a/flightros/params/snaga.yaml +++ b/flightros/params/snaga.yaml @@ -4,7 +4,7 @@ record: single_position: - 1.0 - - 40.0 + - 1.0 - 1.0 single_orientation: - 0.0 diff --git a/flightros/src/record.cpp b/flightros/src/event_camera/record.cpp similarity index 100% rename from flightros/src/record.cpp rename to flightros/src/event_camera/record.cpp diff --git a/flightros/src/ros_utils.cpp b/flightros/src/utils/ros_utils.cpp similarity index 100% rename from flightros/src/ros_utils.cpp rename to flightros/src/utils/ros_utils.cpp diff --git a/flightros/src/rosbag_writer.cpp b/flightros/src/utils/rosbag_writer.cpp similarity index 100% rename from flightros/src/rosbag_writer.cpp rename to flightros/src/utils/rosbag_writer.cpp From 45ff3bd3694e05cb8e2eaa635917538c61d25a7d Mon Sep 17 00:00:00 2001 From: gerni17 Date: Mon, 26 Apr 2021 15:50:04 +0200 Subject: [PATCH 5/7] removed dependencies --- flightros/dependencies.yaml | 28 +------- flightros/include/flightros/rosbag_writer.hpp | 11 ++- flightros/include/flightros/testing.hpp | 9 +-- flightros/package.xml | 5 -- flightros/src/event_camera/record.cpp | 10 +-- flightros/src/test/testing.cpp | 46 ++++++------- flightros/src/utils/rosbag_writer.cpp | 68 +++++++++++-------- 7 files changed, 71 insertions(+), 106 deletions(-) diff --git a/flightros/dependencies.yaml b/flightros/dependencies.yaml index 71e211cdae..4040fd848e 100644 --- a/flightros/dependencies.yaml +++ b/flightros/dependencies.yaml @@ -27,31 +27,7 @@ repositories: type: git url: git@github.com:uzh-rpg/rpg_quadrotor_control.git version: master - glog_catkin: + rpg_dvs_ros: type: git - url: git@github.com:ethz-asl/glog_catkin.git - version: master - ze_oss: - type: git - url: git@github.com:uzh-rpg/ze_oss.git - version: master - gflags_catkin: - type: git - url: git@github.com:ethz-asl/gflags_catkin.git - version: master - eigen_checks: - type: git - url: git@github.com:ethz-asl/eigen_checks.git - version: master - minkindr: - type: git - url: git@github.com:ethz-asl/minkindr.git - version: master - minkindr_ros: - type: git - url: git@github.com:ethz-asl/minkindr_ros.git - version: master - yaml_cpp_catkin: - type: git - url: git@github.com:ethz-asl/yaml_cpp_catkin.git + url: git@github.com:uzh-rpg/rpg_dvs_ros.git version: master \ No newline at end of file diff --git a/flightros/include/flightros/rosbag_writer.hpp b/flightros/include/flightros/rosbag_writer.hpp index 37a13e29d0..42c4cc512c 100644 --- a/flightros/include/flightros/rosbag_writer.hpp +++ b/flightros/include/flightros/rosbag_writer.hpp @@ -1,17 +1,14 @@ #pragma once -#include -#include -#include -#include + #include #include #include #include +#include "flightlib/common/quad_state.hpp" + #include -#include -#include namespace flightros { @@ -27,7 +24,7 @@ class RosbagWriter { void imageOFCallback(const RGBImagePtr& image, int64_t t); void imageDepthCallback(const RGBImagePtr& image, int64_t t); void eventsCallback(const EventsVector& events, int64_t t); - void poseCallback(const ze::Transformation& T_W_C, int64_t t); + void poseCallback(QuadState& quad_state, int64_t t); void imageEventCallback(const RGBImagePtr& images, int64_t t); private: diff --git a/flightros/include/flightros/testing.hpp b/flightros/include/flightros/testing.hpp index dd3eb37be8..0457957b95 100644 --- a/flightros/include/flightros/testing.hpp +++ b/flightros/include/flightros/testing.hpp @@ -43,13 +43,6 @@ #include #include -#include -#include -#include -#include -#include -#include -#include using namespace flightlib; using namespace flightros; @@ -86,7 +79,7 @@ RenderMessage_t unity_output_; uint16_t receive_id_{0}; std::ofstream events_text_file_; int count_; -std::vector values, amount; +// std::vector values, amount; std::vector errors; int num_cam=1; const std::string path_to_output_bag= "/home/gian/bags_flightmare/out.bag"; diff --git a/flightros/package.xml b/flightros/package.xml index 0d1a858f06..39a0b127c8 100644 --- a/flightros/package.xml +++ b/flightros/package.xml @@ -23,16 +23,11 @@ geometry_msgs tf tf_conversions - minkindr_conversions eigen_conversions image_transport cv_bridge - ze_common - ze_matplotlib - ze_cameras rosbag dvs_msgs - mav_msgs \ No newline at end of file diff --git a/flightros/src/event_camera/record.cpp b/flightros/src/event_camera/record.cpp index 6180f48408..ba5a2ae363 100644 --- a/flightros/src/event_camera/record.cpp +++ b/flightros/src/event_camera/record.cpp @@ -408,16 +408,10 @@ int main(int argc, char* argv[]) { line_pub_.publish(desired_path); // define new position and orientation - ze::Transformation twc; record::quad_state_.qx.normalize(); - twc.getRotation() = ze::Quaternion( - record::quad_state_.x[QS::ATTW], record::quad_state_.x[QS::ATTX], - record::quad_state_.x[QS::ATTY], record::quad_state_.x[QS::ATTZ]); - twc.getPosition() = ze::Position(record::quad_state_.x[QS::POSX], - record::quad_state_.x[QS::POSY], - record::quad_state_.x[QS::POSZ]); + // writ position to rosbag - record::writer_->poseCallback(twc, record::event_camera_->getNanoSimTime()); + record::writer_->poseCallback(record::quad_state_, record::event_camera_->getNanoSimTime()); ROS_INFO_STREAM("pose " << record::quad_state_.x[QS::POSX] << "/" << record::quad_state_.x[QS::POSY] << "/" diff --git a/flightros/src/test/testing.cpp b/flightros/src/test/testing.cpp index 0131fb4032..0bb095fc31 100644 --- a/flightros/src/test/testing.cpp +++ b/flightros/src/test/testing.cpp @@ -73,7 +73,7 @@ int main(int argc, char* argv[]) { ros::NodeHandle pnh("~"); ros::Rate(50.0); image_transport::ImageTransport my_image_transport(nh); - std::vector times, mean_errors, stddev_errors; + // std::vector times, mean_errors, stddev_errors; // quad initialization testing::quad_ptr_ = std::make_unique(); @@ -181,14 +181,14 @@ int main(int argc, char* argv[]) { testing::quad_state_.x[QS::ATTY] = (Scalar)desired_pose.orientation.y(); testing::quad_state_.x[QS::ATTZ] = (Scalar)desired_pose.orientation.z(); - ze::Transformation tcv; - testing::quad_state_.qx.normalize(); - tcv.getRotation() = ze::Quaternion( - testing::quad_state_.x[QS::ATTW], testing::quad_state_.x[QS::ATTX], - testing::quad_state_.x[QS::ATTY], testing::quad_state_.x[QS::ATTZ]); - tcv.getPosition() = ze::Position((Scalar)desired_pose.position.x(), - (Scalar)desired_pose.position.y(), - (Scalar)desired_pose.position.z()); + // ze::Transformation tcv; + // testing::quad_state_.qx.normalize(); + // tcv.getRotation() = ze::Quaternion( + // testing::quad_state_.x[QS::ATTW], testing::quad_state_.x[QS::ATTX], + // testing::quad_state_.x[QS::ATTY], testing::quad_state_.x[QS::ATTZ]); + // tcv.getPosition() = ze::Position((Scalar)desired_pose.position.x(), + // (Scalar)desired_pose.position.y(), + // (Scalar)desired_pose.position.z()); testing::quad_ptr_->setState(testing::quad_state_); ROS_INFO_STREAM("time " << testing::event_camera_->getSecSimTime()); @@ -367,26 +367,26 @@ int main(int argc, char* argv[]) { // setup variables for next rendering if (frame < 10 || frame % 1 == 0) L_reconstructed = L.clone(); else if (frame > 10) { - times.push_back(frame); - mean_errors.push_back(mean_error[0]); - stddev_errors.push_back(stddev_error[0]); + // times.push_back(frame); + // mean_errors.push_back(mean_error[0]); + // stddev_errors.push_back(stddev_error[0]); } L_last = L.clone(); time_ += 0.001; frame++; } - // Plot the mean and stddev reconstruction error over time - ze::plt::plot(times, mean_errors, "r"); - ze::plt::plot(times, stddev_errors, "b--"); - std::stringstream title; - title << "C = " << cp << ", sigma = " << testing::event_camera_->getsigmaCm() - << ", bias = " << 0; - ze::plt::title(title.str()); - ze::plt::xlabel("Time (s)"); - ze::plt::ylabel("Mean / Stddev reconstruction error"); - ze::plt::grid(true); - ze::plt::show(); + // // Plot the mean and stddev reconstruction error over time + // ze::plt::plot(times, mean_errors, "r"); + // ze::plt::plot(times, stddev_errors, "b--"); + // std::stringstream title; + // title << "C = " << cp << ", sigma = " << testing::event_camera_->getsigmaCm() + // << ", bias = " << 0; + // ze::plt::title(title.str()); + // ze::plt::xlabel("Time (s)"); + // ze::plt::ylabel("Mean / Stddev reconstruction error"); + // ze::plt::grid(true); + // ze::plt::show(); testing::events_text_file_.close(); return 0; } diff --git a/flightros/src/utils/rosbag_writer.cpp b/flightros/src/utils/rosbag_writer.cpp index 71b4818154..4ac630ad7f 100644 --- a/flightros/src/utils/rosbag_writer.cpp +++ b/flightros/src/utils/rosbag_writer.cpp @@ -1,12 +1,12 @@ #include -DECLARE_double(ros_publisher_camera_info_rate); -DECLARE_double(ros_publisher_frame_rate); -DECLARE_double(ros_publisher_depth_rate); -DECLARE_double(ros_publisher_pointcloud_rate); -DECLARE_double(ros_publisher_optic_flow_rate); -DEFINE_string(path_to_output_bag, "", - "Path to which save the output bag file."); +// DECLARE_double(ros_publisher_camera_info_rate); +// DECLARE_double(ros_publisher_frame_rate); +// DECLARE_double(ros_publisher_depth_rate); +// DECLARE_double(ros_publisher_pointcloud_rate); +// DECLARE_double(ros_publisher_optic_flow_rate); +// DEFINE_string(path_to_output_bag, "", +// "Path to which save the output bag file."); namespace flightros { RosbagWriter::RosbagWriter(const std::string& path_to_output_bag) { @@ -16,8 +16,8 @@ RosbagWriter::RosbagWriter(const std::string& path_to_output_bag) { try { bag_.open(path_to_output_bag, rosbag::bagmode::Write); } catch (rosbag::BagIOException e) { - LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag - << std::endl; + // LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag + // << std::endl; return; } @@ -33,8 +33,8 @@ RosbagWriter::RosbagWriter(const std::string& path_to_output_bag, try { bag_.open(path_to_output_bag, rosbag::bagmode::Write); } catch (rosbag::BagIOException e) { - LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag - << std::endl; + // L return; } last_published_camera_info_time_ = 0; @@ -42,9 +42,9 @@ RosbagWriter::RosbagWriter(const std::string& path_to_output_bag, } RosbagWriter::~RosbagWriter() { - LOG(INFO) << "Finalizing the bag..."; + // LOG(INFO) << "Finalizing the bag..."; bag_.close(); - LOG(INFO) << "Finished writing to bag: " << FLAGS_path_to_output_bag; + // LOG(INFO) << "Finished writing to bag: " << FLAGS_path_to_output_bag; } void RosbagWriter::imageCallback(const ImagePtr& image, int64_t t) { @@ -122,24 +122,34 @@ void RosbagWriter::eventsCallback(const EventsVector& events, int64_t t) { msg); } -void RosbagWriter::poseCallback(const ze::Transformation& T_W_C, int64_t t) { +void RosbagWriter::poseCallback(QuadState& quad_state, int64_t t) { geometry_msgs::PoseStamped pose_stamped_msg; - geometry_msgs::TransformStamped transform_stamped_msg; - transform_stamped_msg.header.frame_id = "map"; - transform_stamped_msg.header.stamp = toRosTime(t + starting_time); - tf::tfMessage tf_msg; + pose_stamped_msg.header.frame_id = "map"; + pose_stamped_msg.header.stamp = toRosTime(t + starting_time); + pose_stamped_msg.pose.orientation.x=quad_state.x[QS::ATTX]; + pose_stamped_msg.pose.orientation.y=quad_state.x[QS::ATTY]; + pose_stamped_msg.pose.orientation.z=quad_state.x[QS::ATTZ]; + pose_stamped_msg.pose.orientation.w=quad_state.x[QS::ATTW]; + + pose_stamped_msg.pose.position.x=quad_state.x[QS::POSX]; + pose_stamped_msg.pose.position.y=quad_state.x[QS::POSY]; + pose_stamped_msg.pose.position.z=quad_state.x[QS::POSZ]; - tf::poseStampedKindrToMsg(T_W_C, toRosTime(t + starting_time), "map", - &pose_stamped_msg); bag_.write(getTopicName(topic_name_prefix_, 0, "pose"), - toRosTime(t + starting_time), pose_stamped_msg); - - // Write tf transform to bag - std::stringstream ss; - ss << "cam"; - transform_stamped_msg.child_frame_id = ss.str(); - tf::transformKindrToMsg(T_W_C, &transform_stamped_msg.transform); - tf_msg.transforms.push_back(transform_stamped_msg); - bag_.write("/tf", toRosTime(t + starting_time), tf_msg); + toRosTime(t + starting_time), pose_stamped_msg); + + // geometry_msgs::TransformStamped transform_stamped_msg; + // tf::tfMessage tf_msg; + // transform_stamped_msg.header.frame_id = "map"; + // transform_stamped_msg.header.stamp = toRosTime(t + starting_time); + // std::stringstream ss; + // ss << "cam"; + // transform_stamped_msg.child_frame_id = ss.str(); + // // tf::transformKindrToMsg(T_W_C, &transform_stamped_msg.transform); + // transform_stamped_msg.transform.translation. + + // tf_msg.transforms.push_back(transform_stamped_msg); + // bag_.write("/tf", toRosTime(t + starting_time), tf_msg); + } } // namespace flightros \ No newline at end of file From 41676f49a8c5e34c192845e8a1f4cf8529540ae1 Mon Sep 17 00:00:00 2001 From: Yunlong Date: Mon, 25 Oct 2021 11:15:24 +0200 Subject: [PATCH 6/7] code cleaning --- flightlib/include/flightlib/common/types.hpp | 2 - .../include/flightlib/objects/quadrotor.hpp | 3 +- .../flightlib/sensors/event_camera.hpp | 2 +- flightlib/src/objects/quadrotor.cpp | 2 +- flightlib/src/sensors/event_camera.cpp | 3 +- flightros/CMakeLists.txt | 26 ++++----- .../flightros/{ => event_camera}/record.hpp | 10 ++-- .../{ => event_camera}/ros_utils.hpp | 31 ++++++----- .../{ => event_camera}/rosbag_writer.hpp | 5 +- .../flightros/{ => event_camera}/testing.hpp | 11 ++-- flightros/src/event_camera/record.cpp | 13 ++--- .../src/{utils => event_camera}/ros_utils.cpp | 2 +- .../{utils => event_camera}/rosbag_writer.cpp | 51 +++++++++--------- flightros/src/event_camera/test.bag | Bin 0 -> 4117 bytes .../src/{test => event_camera}/testing.cpp | 22 ++++---- 15 files changed, 99 insertions(+), 84 deletions(-) rename flightros/include/flightros/{ => event_camera}/record.hpp (92%) rename flightros/include/flightros/{ => event_camera}/ros_utils.hpp (68%) rename flightros/include/flightros/{ => event_camera}/rosbag_writer.hpp (92%) rename flightros/include/flightros/{ => event_camera}/testing.hpp (90%) rename flightros/src/{utils => event_camera}/ros_utils.cpp (97%) rename flightros/src/{utils => event_camera}/rosbag_writer.cpp (78%) create mode 100644 flightros/src/event_camera/test.bag rename flightros/src/{test => event_camera}/testing.cpp (97%) diff --git a/flightlib/include/flightlib/common/types.hpp b/flightlib/include/flightlib/common/types.hpp index 7cbb9be7ce..8efc6ff900 100644 --- a/flightlib/include/flightlib/common/types.hpp +++ b/flightlib/include/flightlib/common/types.hpp @@ -122,6 +122,4 @@ using RGBImagePtr = std::shared_ptr; using RGBImagePtrVector = std::vector; - - } // namespace flightlib diff --git a/flightlib/include/flightlib/objects/quadrotor.hpp b/flightlib/include/flightlib/objects/quadrotor.hpp index d61f36b8d4..ca7b06817d 100644 --- a/flightlib/include/flightlib/objects/quadrotor.hpp +++ b/flightlib/include/flightlib/objects/quadrotor.hpp @@ -44,7 +44,8 @@ class Quadrotor : ObjectBase { std::vector> getEventCameras(void) const; bool getCamera(const size_t cam_id, std::shared_ptr camera) const; - bool getEventCamera(const size_t cam_id, std::shared_ptr camera) const; + bool getEventCamera(const size_t cam_id, + std::shared_ptr camera) const; bool getCollision() const; // public set functions diff --git a/flightlib/include/flightlib/sensors/event_camera.hpp b/flightlib/include/flightlib/sensors/event_camera.hpp index fce3f581b0..496941448b 100644 --- a/flightlib/include/flightlib/sensors/event_camera.hpp +++ b/flightlib/include/flightlib/sensors/event_camera.hpp @@ -75,7 +75,7 @@ class EventCamera : SensorBase { cv::Mat createEventimages(); private: - Logger logger_{"RBGCamera"}; + Logger logger_{"EventCamera"}; // camera parameters int channels_; diff --git a/flightlib/src/objects/quadrotor.cpp b/flightlib/src/objects/quadrotor.cpp index 109a945e63..7f3f05dfb5 100644 --- a/flightlib/src/objects/quadrotor.cpp +++ b/flightlib/src/objects/quadrotor.cpp @@ -240,7 +240,7 @@ bool Quadrotor::addRGBCamera(std::shared_ptr camera) { return true; } - bool Quadrotor::addEventCamera(std::shared_ptr camera){ +bool Quadrotor::addEventCamera(std::shared_ptr camera) { event_cameras_.push_back(camera); return true; } diff --git a/flightlib/src/sensors/event_camera.cpp b/flightlib/src/sensors/event_camera.cpp index 7e3027d623..4f47c97f47 100644 --- a/flightlib/src/sensors/event_camera.cpp +++ b/flightlib/src/sensors/event_camera.cpp @@ -34,7 +34,8 @@ bool EventCamera::setRelPose(const Ref> B_r_BC, const Ref> R_BC) { if (!B_r_BC.allFinite() || !R_BC.allFinite()) { logger_.error( - "The setting value for Camera Relative Pose Matrix is not valid, discard " + "The setting value for Camera Relative Pose Matrix is not valid, " + "discard " "the setting."); return false; } diff --git a/flightros/CMakeLists.txt b/flightros/CMakeLists.txt index b0a881e691..4d63e73b44 100644 --- a/flightros/CMakeLists.txt +++ b/flightros/CMakeLists.txt @@ -44,7 +44,7 @@ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -Wall -g") include_directories(include) cs_add_library(ros_utils - src/utils/ros_utils.cpp) + src/event_camera/ros_utils.cpp) #pilot cs_add_library(flight_pilot @@ -68,11 +68,11 @@ target_link_libraries(flight_pilot_node ) cs_add_library(rosbag_writer -src/utils/rosbag_writer.cpp +src/event_camera/rosbag_writer.cpp ) cs_add_executable(testing -src/test/testing.cpp) +src/event_camera/testing.cpp) target_link_libraries(testing ${catkin_LIBRARIES} @@ -169,11 +169,11 @@ cs_add_executable(racing ) target_link_libraries(racing -${catkin_LIBRARIES} -${OpenCV_LIBRARIES} -stdc++fs -zmq -zmqpp + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + stdc++fs + zmq + zmqpp ) # camera @@ -183,11 +183,11 @@ cs_add_executable(camera ) target_link_libraries(camera -${catkin_LIBRARIES} -${OpenCV_LIBRARIES} -stdc++fs -zmq -zmqpp + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + stdc++fs + zmq + zmqpp ) # Finish diff --git a/flightros/include/flightros/record.hpp b/flightros/include/flightros/event_camera/record.hpp similarity index 92% rename from flightros/include/flightros/record.hpp rename to flightros/include/flightros/event_camera/record.hpp index 79ee235750..34e2396dcf 100644 --- a/flightros/include/flightros/record.hpp +++ b/flightros/include/flightros/event_camera/record.hpp @@ -7,8 +7,6 @@ #include #include #include -#include "flightros/ros_utils.hpp" -#include "flightros/rosbag_writer.hpp" #include "opencv2/imgcodecs.hpp" @@ -35,6 +33,9 @@ #include "flightlib/sensors/event_camera.hpp" #include "flightlib/sensors/rgb_camera.hpp" +#include "flightros/event_camera/ros_utils.hpp" +#include "flightros/event_camera/rosbag_writer.hpp" + // trajectory #include #include @@ -84,7 +85,10 @@ std::ofstream events_text_file_; int count_; std::vector errors; int num_cam = 1; -std::string path_to_output_bag = "/home/gian/bags_flightmare/record.bag"; +std::string path_to_output_bag = + getenv("FLIGHTMARE_PATH") + + std::string("/flightros/src/event_camera/record.bag"); +; std::shared_ptr writer_; } // namespace record \ No newline at end of file diff --git a/flightros/include/flightros/ros_utils.hpp b/flightros/include/flightros/event_camera/ros_utils.hpp similarity index 68% rename from flightros/include/flightros/ros_utils.hpp rename to flightros/include/flightros/event_camera/ros_utils.hpp index 878e89becf..8acf89e4ee 100644 --- a/flightros/include/flightros/ros_utils.hpp +++ b/flightros/include/flightros/event_camera/ros_utils.hpp @@ -1,40 +1,43 @@ #pragma once -#include -#include -#include #include #include #include -#include +#include #include +#include +#include + +// +#include "flightlib/common/types.hpp" using namespace flightlib; namespace flightros { -inline std::string getTopicName(int i, const std::string& suffix) -{ +inline std::string getTopicName(int i, const std::string& suffix) { std::stringstream ss; ss << "cam" << i << "/" << suffix; return ss.str(); } -inline std::string getTopicName(const std::string& prefix, int i, const std::string& suffix) -{ +inline std::string getTopicName(const std::string& prefix, int i, + const std::string& suffix) { std::stringstream ss; ss << prefix << "/" << getTopicName(i, suffix); return ss.str(); } -inline ros::Time toRosTime(int64_t t) -{ +inline ros::Time toRosTime(int64_t t) { ros::Time ros_time; ros_time.fromNSec(t); return ros_time; } -void imageToMsg(const cv::Mat_& image, int64_t t, sensor_msgs::ImagePtr& msg); +void imageToMsg(const cv::Mat_& image, int64_t t, + sensor_msgs::ImagePtr& msg); void imageToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg); -void imageFloatToMsg(const cv::Mat& image, int64_t t, sensor_msgs::ImagePtr& msg); -void eventsToMsg(const EventsVector& events, int width, int height, dvs_msgs::EventArrayPtr& msg, int64_t starting_time); +void imageFloatToMsg(const cv::Mat& image, int64_t t, + sensor_msgs::ImagePtr& msg); +void eventsToMsg(const EventsVector& events, int width, int height, + dvs_msgs::EventArrayPtr& msg, int64_t starting_time); -} // namespace flightros +} // namespace flightros diff --git a/flightros/include/flightros/rosbag_writer.hpp b/flightros/include/flightros/event_camera/rosbag_writer.hpp similarity index 92% rename from flightros/include/flightros/rosbag_writer.hpp rename to flightros/include/flightros/event_camera/rosbag_writer.hpp index 42c4cc512c..f2b2d8e9ef 100644 --- a/flightros/include/flightros/rosbag_writer.hpp +++ b/flightros/include/flightros/event_camera/rosbag_writer.hpp @@ -5,10 +5,9 @@ #include #include -#include #include "flightlib/common/quad_state.hpp" - -#include +#include "flightlib/common/types.hpp" +#include "flightros/event_camera/ros_utils.hpp" namespace flightros { diff --git a/flightros/include/flightros/testing.hpp b/flightros/include/flightros/event_camera/testing.hpp similarity index 90% rename from flightros/include/flightros/testing.hpp rename to flightros/include/flightros/event_camera/testing.hpp index 0457957b95..36e3d06a53 100644 --- a/flightros/include/flightros/testing.hpp +++ b/flightros/include/flightros/event_camera/testing.hpp @@ -8,8 +8,6 @@ #include #include #include "opencv2/imgcodecs.hpp" -#include "flightros/rosbag_writer.hpp" -#include "flightros/ros_utils.hpp" // standard libraries @@ -42,6 +40,9 @@ #include #include +#include "flightros/event_camera/ros_utils.hpp" +#include "flightros/event_camera/rosbag_writer.hpp" + #include using namespace flightlib; @@ -81,7 +82,9 @@ std::ofstream events_text_file_; int count_; // std::vector values, amount; std::vector errors; -int num_cam=1; -const std::string path_to_output_bag= "/home/gian/bags_flightmare/out.bag"; +int num_cam = 1; +const std::string path_to_output_bag = + getenv("FLIGHTMARE_PATH") + + std::string("/flightros/src/event_camera/test.bag"); std::shared_ptr writer_; } // namespace testing diff --git a/flightros/src/event_camera/record.cpp b/flightros/src/event_camera/record.cpp index ba5a2ae363..bade51adb0 100644 --- a/flightros/src/event_camera/record.cpp +++ b/flightros/src/event_camera/record.cpp @@ -1,4 +1,4 @@ -#include "flightros/record.hpp" +#include "flightros/event_camera/record.hpp" bool record::setUnity(const bool render) { unity_render_ = render; @@ -90,7 +90,7 @@ void record::createMinSnap(const std::vector waypoints, // identify the segment times Eigen::VectorXd segment_times = Eigen::VectorXd::Ones(waypoints.size() - 1); double desired_speed = 1.0; - for (int i = 0; i < waypoints.size() - 1; i++) { + for (int i = 0; i < (int)waypoints.size() - 1; i++) { segment_times[i] = (waypoints.at(i + 1) - waypoints.at(i)).norm() / desired_speed; ROS_INFO_STREAM("seg time" << segment_times[i]); @@ -131,7 +131,7 @@ polynomial_trajectories::PolynomialTrajectory record::createOwnSnap( const std::vector waypoints_in, Eigen::VectorXd segment_times_in) { double desired_speed_in = 1.0; - for (int i = 0; i < waypoints_in.size() - 1; i++) { + for (int i = 0; i < (int)waypoints_in.size() - 1; i++) { segment_times_in[i] = (waypoints_in.at(i + 1) - waypoints_in.at(i)).norm() / desired_speed_in; ROS_INFO_STREAM("seg time: " << segment_times_in[i]); @@ -316,11 +316,11 @@ int main(int argc, char* argv[]) { generateMinimumSnapRingTrajectory(segment_times, trajectory_settings, 20.0, 20.0, 6.0); } else { - for (int it = 0; it < waypts_x.size(); it++) { + for (int it = 0; it < (int)waypts_x.size(); it++) { way_points.push_back( Eigen::Vector3d(waypts_x.at(it), waypts_y.at(it), waypts_z.at(it))); } - for (int it = 0; it < waypts_x.size() - 1; it++) { + for (int it = 0; it < (int)waypts_x.size() - 1; it++) { seg_times[it] = 10.0; } } @@ -411,7 +411,8 @@ int main(int argc, char* argv[]) { record::quad_state_.qx.normalize(); // writ position to rosbag - record::writer_->poseCallback(record::quad_state_, record::event_camera_->getNanoSimTime()); + record::writer_->poseCallback(record::quad_state_, + record::event_camera_->getNanoSimTime()); ROS_INFO_STREAM("pose " << record::quad_state_.x[QS::POSX] << "/" << record::quad_state_.x[QS::POSY] << "/" diff --git a/flightros/src/utils/ros_utils.cpp b/flightros/src/event_camera/ros_utils.cpp similarity index 97% rename from flightros/src/utils/ros_utils.cpp rename to flightros/src/event_camera/ros_utils.cpp index 24e5111933..d91a7af27c 100644 --- a/flightros/src/utils/ros_utils.cpp +++ b/flightros/src/event_camera/ros_utils.cpp @@ -1,6 +1,6 @@ #include -#include +#include "flightros/event_camera/ros_utils.hpp" namespace flightros { diff --git a/flightros/src/utils/rosbag_writer.cpp b/flightros/src/event_camera/rosbag_writer.cpp similarity index 78% rename from flightros/src/utils/rosbag_writer.cpp rename to flightros/src/event_camera/rosbag_writer.cpp index 4ac630ad7f..b5776d237a 100644 --- a/flightros/src/utils/rosbag_writer.cpp +++ b/flightros/src/event_camera/rosbag_writer.cpp @@ -1,4 +1,4 @@ -#include +#include "flightros/event_camera/rosbag_writer.hpp" // DECLARE_double(ros_publisher_camera_info_rate); // DECLARE_double(ros_publisher_frame_rate); @@ -13,13 +13,16 @@ RosbagWriter::RosbagWriter(const std::string& path_to_output_bag) { num_cameras_ = 1; sensor_size_ = cv::Size(100, 100); starting_time = 1; - try { - bag_.open(path_to_output_bag, rosbag::bagmode::Write); - } catch (rosbag::BagIOException e) { - // LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag - // << std::endl; - return; - } + // try { + bag_.open(path_to_output_bag, rosbag::bagmode::Write); + std::cout << "Successfully open the ros bag. " << path_to_output_bag + << std::endl; + // } catch (rosbag::BagIOException e) { + // // LOG(FATAL) << "Error: could not open rosbag: " << + // // FLAGS_path_to_output_bag + // // << std::endl; + // return; + // } last_published_camera_info_time_ = 0; last_published_image_time_ = 0; @@ -30,13 +33,14 @@ RosbagWriter::RosbagWriter(const std::string& path_to_output_bag, sensor_size_ = cv::Size(100, 100); starting_time = 1; // stime - try { - bag_.open(path_to_output_bag, rosbag::bagmode::Write); - } catch (rosbag::BagIOException e) { - // L - return; - } + // try { + bag_.open(path_to_output_bag, rosbag::bagmode::Write); + // } catch (rosbag::BagIOException e) { + // // L + // return; + // } last_published_camera_info_time_ = 0; last_published_image_time_ = 0; } @@ -126,17 +130,17 @@ void RosbagWriter::poseCallback(QuadState& quad_state, int64_t t) { geometry_msgs::PoseStamped pose_stamped_msg; pose_stamped_msg.header.frame_id = "map"; pose_stamped_msg.header.stamp = toRosTime(t + starting_time); - pose_stamped_msg.pose.orientation.x=quad_state.x[QS::ATTX]; - pose_stamped_msg.pose.orientation.y=quad_state.x[QS::ATTY]; - pose_stamped_msg.pose.orientation.z=quad_state.x[QS::ATTZ]; - pose_stamped_msg.pose.orientation.w=quad_state.x[QS::ATTW]; + pose_stamped_msg.pose.orientation.x = quad_state.x[QS::ATTX]; + pose_stamped_msg.pose.orientation.y = quad_state.x[QS::ATTY]; + pose_stamped_msg.pose.orientation.z = quad_state.x[QS::ATTZ]; + pose_stamped_msg.pose.orientation.w = quad_state.x[QS::ATTW]; - pose_stamped_msg.pose.position.x=quad_state.x[QS::POSX]; - pose_stamped_msg.pose.position.y=quad_state.x[QS::POSY]; - pose_stamped_msg.pose.position.z=quad_state.x[QS::POSZ]; + pose_stamped_msg.pose.position.x = quad_state.x[QS::POSX]; + pose_stamped_msg.pose.position.y = quad_state.x[QS::POSY]; + pose_stamped_msg.pose.position.z = quad_state.x[QS::POSZ]; bag_.write(getTopicName(topic_name_prefix_, 0, "pose"), - toRosTime(t + starting_time), pose_stamped_msg); + toRosTime(t + starting_time), pose_stamped_msg); // geometry_msgs::TransformStamped transform_stamped_msg; // tf::tfMessage tf_msg; @@ -150,6 +154,5 @@ void RosbagWriter::poseCallback(QuadState& quad_state, int64_t t) { // tf_msg.transforms.push_back(transform_stamped_msg); // bag_.write("/tf", toRosTime(t + starting_time), tf_msg); - } } // namespace flightros \ No newline at end of file diff --git a/flightros/src/event_camera/test.bag b/flightros/src/event_camera/test.bag new file mode 100644 index 0000000000000000000000000000000000000000..7d16626fde9b87a2bd6a1082d3309aec549fb005 GIT binary patch literal 4117 zcmY!m@(*@$bXN#7(lg+4Wnf?s0OI6~(!A{WsetPostProcesscing( std::vector{true, false, true}); // depth, segmentation, optical flow testing::quad_ptr_->addRGBCamera(testing::rgb_camera_); - + // initialize event camera, careful with the image size. - // h,w should be a mutliple of 8 otherwise the thread groups in the compute + // h,w should be a mutliple of 8 otherwise the thread groups in the compute // shader need to be adapted testing::event_camera_->setFOV(83); testing::event_camera_->setWidth(512); @@ -109,7 +109,7 @@ int main(int argc, char* argv[]) { testing::event_camera_->setLogEps(0.0001); testing::quad_ptr_->addEventCamera(testing::event_camera_); - // scene can be changed + // scene can be changed // testing::scene_id_ = 1; double cp = testing::event_camera_->getCp(); double cm = testing::event_camera_->getCm(); @@ -136,7 +136,7 @@ int main(int argc, char* argv[]) { // connect unity testing::connectUnity(); - // Define path + // Define path std::vector way_points; way_points.push_back(Eigen::Vector3d(0, 10, 2.5)); way_points.push_back(Eigen::Vector3d(5, 0, 2.5)); @@ -168,7 +168,6 @@ int main(int argc, char* argv[]) { float time_ = 0; while (ros::ok() && testing::unity_render_ && testing::unity_ready_ && frame < 200) { - quadrotor_common::TrajectoryPoint desired_pose = polynomial_trajectories::getPointFromTrajectory(trajectory, ros::Duration(time_)); @@ -195,7 +194,7 @@ int main(int argc, char* argv[]) { testing::unity_bridge_ptr_->getRender(0); testing::unity_bridge_ptr_->handleOutput(true); - //make some checks + // make some checks cv::Mat new_image, rgb_img, of_img, depth_img; testing::event_camera_->getRGBImage(new_image); @@ -229,7 +228,8 @@ int main(int argc, char* argv[]) { cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg(); testing::depth_pub_.publish(depth_msg); - ROS_INFO_STREAM("recieved OF and depth " << was_of_empty << " / "<< was_depth_empty); + ROS_INFO_STREAM("recieved OF and depth " << was_of_empty << " / " + << was_depth_empty); ROS_INFO_STREAM("new image type " << testing::type2str(of_img.type())); // coonvert rgb image to gray @@ -365,7 +365,8 @@ int main(int argc, char* argv[]) { << max.x); // setup variables for next rendering - if (frame < 10 || frame % 1 == 0) L_reconstructed = L.clone(); + if (frame < 10 || frame % 1 == 0) + L_reconstructed = L.clone(); else if (frame > 10) { // times.push_back(frame); // mean_errors.push_back(mean_error[0]); @@ -380,7 +381,8 @@ int main(int argc, char* argv[]) { // ze::plt::plot(times, mean_errors, "r"); // ze::plt::plot(times, stddev_errors, "b--"); // std::stringstream title; - // title << "C = " << cp << ", sigma = " << testing::event_camera_->getsigmaCm() + // title << "C = " << cp << ", sigma = " << + // testing::event_camera_->getsigmaCm() // << ", bias = " << 0; // ze::plt::title(title.str()); // ze::plt::xlabel("Time (s)"); From 3a2ce1de26d88aad32120329359085b4ae235015 Mon Sep 17 00:00:00 2001 From: Yunlong Date: Mon, 25 Oct 2021 11:28:44 +0200 Subject: [PATCH 7/7] code cleaning --- .gitignore | 4 +- flightlib/src/bridges/unity_bridge.cpp | 53 +++++++++++++------------- flightros/params/local.yaml | 10 +++++ flightros/src/event_camera/record.cpp | 1 - flightros/src/racing/racing.cpp | 1 - 5 files changed, 38 insertions(+), 31 deletions(-) create mode 100644 flightros/params/local.yaml diff --git a/.gitignore b/.gitignore index 4e9556d7a2..23ca7190b7 100644 --- a/.gitignore +++ b/.gitignore @@ -3,9 +3,9 @@ dist *.egg-info *.so __pycache__ -build +#build -flightros/params/local.yaml +#flightros/params/local.yaml # Gitignore diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp index ca973351d1..4013b668da 100644 --- a/flightlib/src/bridges/unity_bridge.cpp +++ b/flightlib/src/bridges/unity_bridge.cpp @@ -246,7 +246,7 @@ bool UnityBridge::addStaticObject(std::shared_ptr static_object) { bool UnityBridge::handleOutput() { bool always = false; - handleOutput(always); + return handleOutput(always); } bool UnityBridge::handleOutput(bool always) { @@ -391,32 +391,31 @@ bool UnityBridge::handleOutput(bool always) { bool UnityBridge::getPointCloud(PointCloudMessage_t& pointcloud_msg, Scalar time_out) { - // // create new message object - // zmqpp::message msg; - // // add topic header - // msg << "PointCloud"; - // // create JSON object for initial settings - // json json_msg = pointcloud_msg; - // msg << json_msg.dump(); - // // send message without blocking - // pub_.send(msg, true); - - // std::cout << "Generate PointCloud: Timeout=" << (int)time_out << " - // seconds." - // << std::endl; - - // Scalar run_time = 0.0; - // while (!std::experimental::filesystem::exists( - // pointcloud_msg.path + pointcloud_msg.file_name + ".ply")) { - // if (run_time >= time_out) { - // logger_.warn("Timeout... PointCloud was not saved within expected - // time."); return false; - // } - // std::cout << "Waiting for Pointcloud: Current Runtime=" << (int)run_time - // << " seconds." << std::endl; - // usleep((time_out / 10.0) * 1e6); - // run_time += time_out / 10.0; - // } + // create new message object + zmqpp::message msg; + // add topic header + msg << "PointCloud"; + // create JSON object for initial settings + json json_msg = pointcloud_msg; + msg << json_msg.dump(); + // send message without blocking + pub_.send(msg, true); + + std::cout << "Generate PointCloud: Timeout=" << (int)time_out << "seconds." + << std::endl; + + Scalar run_time = 0.0; + while (!std::experimental::filesystem::exists( + pointcloud_msg.path + pointcloud_msg.file_name + ".ply")) { + if (run_time >= time_out) { + logger_.warn("Timeout... PointCloud was not saved within expected time."); + return false; + } + std::cout << "Waiting for Pointcloud: Current Runtime=" << (int)run_time + << " seconds." << std::endl; + usleep((time_out / 10.0) * 1e6); + run_time += time_out / 10.0; + } return true; } diff --git a/flightros/params/local.yaml b/flightros/params/local.yaml new file mode 100644 index 0000000000..e4cb16648b --- /dev/null +++ b/flightros/params/local.yaml @@ -0,0 +1,10 @@ +rosbag: + trajectory: "/record/japan_six" + rotate: false + trajectory_rotation: false + position_trajectory: true + scene_id: 4 + ring_traj: false + cp: 0.2 + cm: 0.2 + time_for_rotation: 17 diff --git a/flightros/src/event_camera/record.cpp b/flightros/src/event_camera/record.cpp index bade51adb0..01187200b6 100644 --- a/flightros/src/event_camera/record.cpp +++ b/flightros/src/event_camera/record.cpp @@ -204,7 +204,6 @@ int main(int argc, char* argv[]) { pnh.getParam("/rosbag/rotate", rotate); pnh.getParam("/rosbag/trajectory_rotation", trajectory_rotation); pnh.getParam("/rosbag/position_trajectory", position_trajectory); - pnh.getParam("/rosbag/path", record::path_to_output_bag); pnh.getParam("/rosbag/ring_traj", ring_traj); pnh.getParam("/rosbag/scene_id", scene); pnh.getParam("/rosbag/trajectory", which_trajectory); diff --git a/flightros/src/racing/racing.cpp b/flightros/src/racing/racing.cpp index c41827abee..e2873b54c4 100644 --- a/flightros/src/racing/racing.cpp +++ b/flightros/src/racing/racing.cpp @@ -32,7 +32,6 @@ int main(int argc, char *argv[]) { // Flightmare(Unity3D) std::shared_ptr unity_bridge_ptr = UnityBridge::getInstance(); SceneID scene_id{UnityScene::WAREHOUSE}; - scene_id = 4; bool unity_ready{false}; // unity quadrotor