pull request fixes

.gitignore

@@ -10,3 +10,4 @@ dist
 *.so
 __pycache__
 build
+_build

README.md

@@ -23,11 +23,11 @@ Installation instructions can be found in our [Wiki](https://github.com/uzh-rpg/
 If you use this code in a publication, please cite the following paper **[PDF](https://arxiv.org/abs/2009.00563)**
 
 ```
-@article{yunlong2020flightmare,
-  title={Flightmare: A Flexible Quadrotor Simulator},
-  author={Song, Yunlong and Naji, Selim and Kaufmann, Elia and Loquercio, Antonio and Scaramuzza, Davide},
-  journal={arXiv preprint arXiv:2009.00563},
-  year={2020}
+@article{song2020flightmare,
+    title={Flightmare: A Flexible Quadrotor Simulator},
+    author={Song, Yunlong and Naji, Selim and Kaufmann, Elia and Loquercio, Antonio and Scaramuzza, Davide},
+    booktitle={Conference on Robot Learning},
+    year={2020}
 }
 ```
 

flightlib/include/flightlib/bridges/unity_bridge.hpp

@@ -6,7 +6,6 @@
 
 // std libs
 #include <unistd.h>
-#include <chrono>
 #include <experimental/filesystem>
 #include <fstream>
 #include <map>

flightros/CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin_simple REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
-option(BUILD_SAMPLES "Building the samples" OFF)
+option(BUILD_MP "Build Motion Planning" OFF)
 
 catkin_simple()
 
@@ -29,8 +29,6 @@ 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")
@@ -60,12 +58,7 @@ target_link_libraries(flight_pilot_node
 
 # motion_planning
 
-if(BUILD_SAMPLES)
-
-    # find all open3D stuff
-    message("Searching open3D in /usr/local/lib/cmake/")
-    find_package(Open3D HINTS /usr/local/lib/cmake/)
-    list(APPEND Open3D_LIBRARIES dl)
+if(BUILD_MP)
 
     find_package(OpenMP)
     if (OpenMP_CXX_FOUND)
@@ -75,81 +68,85 @@ if(BUILD_SAMPLES)
         foreach (_variableName ${_variableNames})
             message(STATUS "${_variableName}=${${_variableName}}")
         endforeach()
+    else ()
+        message("OpenMP not found")
     endif ()
 
     # Open3D
+    find_package(Open3D)
     if (Open3D_FOUND)
         message("Found Open3D ${Open3D_VERSION}")
+        list(APPEND Open3D_LIBRARIES dl)
         # link_directories must be before add_executable
         link_directories(${Open3D_LIBRARY_DIRS})
     else ()
         message("Open3D not found")
     endif ()
 
     find_package(ompl REQUIRED)
+    if (OMPL_FOUND)
+        message("Found OMPL ${OMPL_VERSION}")
+        include_directories(${OMPL_INCLUDE_DIRS})
 
-    link_directories(/usr/local/lib)
-    include_directories(${OMPL_INCLUDE_DIRS})
-
-    cs_add_executable(motion_planning
-        src/motion_planning/motion_planning.cpp
-    )
-
-    target_include_directories(motion_planning PUBLIC ${Open3D_INCLUDE_DIRS})
+     else ()
+        message("OMPL not found")
+    endif ()
 
-    target_link_libraries(motion_planning
-    ${catkin_LIBRARIES}
-    ${OpenCV_LIBRARIES}
-    stdc++fs
-    ompl
-    ${Open3D_LIBRARIES}
-    OpenMP::OpenMP_CXX
-    zmq
-    zmqpp
-    )
+    if (OpenMP_CXX_FOUND AND Open3D_FOUND AND OMPL_FOUND)
+        cs_add_executable(motion_planning
+            src/motion_planning/motion_planning.cpp
+        )
+        target_include_directories(motion_planning PUBLIC ${Open3D_INCLUDE_DIRS})
+
+        target_link_libraries(motion_planning
+        ${catkin_LIBRARIES}
+        ${OpenCV_LIBRARIES}
+        stdc++fs
+        ompl
+        ${Open3D_LIBRARIES}
+        OpenMP::OpenMP_CXX
+        zmq
+        zmqpp
+        )
+    else ()
+        message("Failed to build motion planning")
+    endif ()
 
 endif()
 
 # racing
 
-if(BUILD_SAMPLES)
-
-    catkin_package(
-    LIBRARIES
-    CATKIN_DEPENDS
-    )
 
-    cs_add_executable(racing
-        src/racing/racing.cpp
-    )
+catkin_package(
+LIBRARIES
+CATKIN_DEPENDS
+)
 
-    target_link_libraries(racing
-    ${catkin_LIBRARIES}
-    ${OpenCV_LIBRARIES}
-    stdc++fs
-    zmq
-    zmqpp
-    )
+cs_add_executable(racing
+    src/racing/racing.cpp
+)
 
-endif()
+target_link_libraries(racing
+${catkin_LIBRARIES}
+${OpenCV_LIBRARIES}
+stdc++fs
+zmq
+zmqpp
+)
 
 # camera
 
-if(BUILD_SAMPLES)
-
-    cs_add_executable(camera
-        src/camera/camera.cpp
-    )
-
-    target_link_libraries(camera
-    ${catkin_LIBRARIES}
-    ${OpenCV_LIBRARIES}
-    stdc++fs
-    zmq
-    zmqpp
-    )
+cs_add_executable(camera
+    src/camera/camera.cpp
+)
 
-endif()
+target_link_libraries(camera
+${catkin_LIBRARIES}
+${OpenCV_LIBRARIES}
+stdc++fs
+zmq
+zmqpp
+)
 
 # Finish
 cs_install()

flightros/include/flightros/camera/camera.hpp

@@ -1,4 +1,3 @@
-#pragma once
 // ros
 #include <cv_bridge/cv_bridge.h>
 #include <image_transport/image_transport.h>
@@ -7,6 +6,7 @@
 // standard libraries
 #include <assert.h>
 #include <Eigen/Dense>
+#include <chrono>
 #include <cmath>
 #include <cstring>
 #include <experimental/filesystem>
@@ -34,7 +34,6 @@ image_transport::Publisher rgb_pub_;
 image_transport::Publisher depth_pub_;
 image_transport::Publisher segmentation_pub_;
 image_transport::Publisher opticalflow_pub_;
-int counter = 0;
 
 // void setupQuad();
 bool setUnity(const bool render);

flightros/include/flightros/motion_planning/motion_planning.hpp

@@ -1,9 +1,6 @@
-#pragma once
-
 // standard libraries
 #include <assert.h>
 #include <Eigen/Dense>
-#include <chrono>
 #include <cmath>
 #include <cstring>
 #include <experimental/filesystem>
@@ -48,21 +45,6 @@ using namespace flightlib;
 
 namespace motion_planning {
 
-class manual_timer {
-  std::chrono::high_resolution_clock::time_point t0;
-  double timestamp{0.0};
-
- public:
-  void start() { t0 = std::chrono::high_resolution_clock::now(); }
-  void stop() {
-    timestamp = std::chrono::duration<double>(
-                  std::chrono::high_resolution_clock::now() - t0)
-                  .count() *
-                1000.0;
-  }
-  const double &get() { return timestamp; }
-};
-
 struct float2 {
   float x, y;
 };

flightros/include/flightros/racing/racing.hpp

@@ -1,5 +1,3 @@
-#pragma once
-
 // ros
 #include <cv_bridge/cv_bridge.h>
 #include <image_transport/image_transport.h>
@@ -38,21 +36,6 @@ using namespace flightlib;
 
 namespace racing {
 
-class manual_timer {
-  std::chrono::high_resolution_clock::time_point t0;
-  double timestamp{0.0};
-
- public:
-  void start() { t0 = std::chrono::high_resolution_clock::now(); }
-  void stop() {
-    timestamp = std::chrono::duration<double>(
-                  std::chrono::high_resolution_clock::now() - t0)
-                  .count() *
-                1000.0;
-  }
-  const double &get() { return timestamp; }
-};
-
 // void setupQuad();
 bool setUnity(const bool render);
 bool connectUnity(void);

flightros/src/camera/camera.cpp

@@ -75,31 +75,31 @@ int main(int argc, char *argv[]) {
 
     cv::Mat img;
 
+    ros::Time timestamp = ros::Time::now();
+
     camera::rgb_camera_->getRGBImage(img);
     sensor_msgs::ImagePtr rgb_msg =
       cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
-    rgb_msg->header.stamp.fromNSec(camera::counter);
+    rgb_msg->header.stamp = timestamp;
     camera::rgb_pub_.publish(rgb_msg);
 
     camera::rgb_camera_->getDepthMap(img);
     sensor_msgs::ImagePtr depth_msg =
       cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
-    depth_msg->header.stamp.fromNSec(camera::counter);
+    depth_msg->header.stamp = timestamp;
     camera::depth_pub_.publish(depth_msg);
 
     camera::rgb_camera_->getSegmentation(img);
     sensor_msgs::ImagePtr segmentation_msg =
       cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
-    segmentation_msg->header.stamp.fromNSec(camera::counter);
+    segmentation_msg->header.stamp = timestamp;
     camera::segmentation_pub_.publish(segmentation_msg);
 
     camera::rgb_camera_->getOpticalFlow(img);
     sensor_msgs::ImagePtr opticflow_msg =
       cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
-    opticflow_msg->header.stamp.fromNSec(camera::counter);
+    opticflow_msg->header.stamp = timestamp;
     camera::opticalflow_pub_.publish(opticflow_msg);
-
-    camera::counter++;
   }
 
   return 0;