Merge pull request #1 from rislab/develop

Added changes from develop to PR uzh-rpg#125

README.md

@@ -26,7 +26,7 @@ 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](http://rpg.ifi.uzh.ch/docs/CoRL20_Yunlong.pdf)**
 
 ```
-@article{song2020flightmare,
+@inproceedings{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},

docs/source/conf.py

@@ -62,8 +62,8 @@
 # so a file named "default.css" will overwrite the builtin "default.css".
 html_static_path = ['_static']
 
-html_context = {
-    'css_files': [
-        '_static/theme_overrides.css',  # override wide tables in RTD theme
-    ],
-}
+# html_context = {
+#     'css_files': [
+#         '_static/theme_overrides.css',  # override wide tables in RTD theme
+#     ],
+# }

flightlib/CMakeLists.txt

@@ -25,12 +25,12 @@ set(
 
 message(STATUS "======> Setup Dependencies")
 if(EIGEN_FROM_SYSTTEM)
-  find_package(Eigen3 3.3.4 QUIET)
+  find_package(Eigen3 3.4.0 EXACT REQUIRED)
   if(EIGEN3_FOUND)
     message(STATUS "Using system provided Eigen.")
     message(${EIGEN3_INCLUDE_DIR})
   else()
-    message(STATUS "No sufficient Eigen version (3.3.4) found.")
+    message(STATUS "No sufficient Eigen version (3.4.0) found.")
     message(STATUS "Restoring to download Eigen sources.")
     include(cmake/eigen.cmake)
   endif()
@@ -84,7 +84,7 @@ endif()
 message(STATUS "======> Setup Compilation")
 
 add_definitions(-DEIGEN_STACK_ALLOCATION_LIMIT=1048576)
-include_directories(${EIGEN_INCLUDE_DIR} "tests")
+include_directories(${EIGEN3_INCLUDE_DIR} "tests")
 include_directories(${OpenCV_INCLUDE_DIRS})
 
 # Set default build type
@@ -159,14 +159,14 @@ file(GLOB_RECURSE FLIGHTLIB_TEST_SOURCES
 )
 
 # Create file lists for flightlib_gym source 
-file(GLOB_RECURSE FLIGHTLIB_GYM_SOURCES
-  src/wrapper/*.cpp 
-)
+# file(GLOB_RECURSE FLIGHTLIB_GYM_SOURCES
+#   src/wrapper/*.cpp 
+# )
 
 # Create file lists for flightlib_gym tests
-file(GLOB_RECURSE FLIGHTLIB_GYM_TEST_SOURCES
-  tests/wrapper/*.cpp
-)
+# file(GLOB_RECURSE FLIGHTLIB_GYM_TEST_SOURCES
+#   tests/wrapper/*.cpp
+# )
 
 # Create file lists for flightlib_unity_bridge tests
 file(GLOB_RECURSE FLIGHTLIB_UNITY_BRIDGE_TEST_SOURCES
@@ -211,25 +211,25 @@ else()
   set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
 endif()
 
-if(FLIGHTLIB_GYM_SOURCES)
-  # flightlib_gym (python3 binding with Pybind11)
-  pybind11_add_module(flightgym MODULE 
-    ${FLIGHTLIB_GYM_SOURCES})
-  if(EIGEN3_FOUND)
-    target_include_directories(flightgym PRIVATE
-      # ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
-      ${PYBIND11_INCLUDE_DIR}
-      ${PROJECT_SOURCE_DIR}/include
-      ${EIGEN3_INCLUDE_DIR})  
-  else()
-    target_include_directories(flightgym PRIVATE
-      # ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
-      ${PYBIND11_INCLUDE_DIR}
-      ${PROJECT_SOURCE_DIR}/externals/eigen/eigen3 #pybind11 use #include <Eigen/Core>, however, flightmare use #include <eigen3/Eigen/Core>
-      ${PROJECT_SOURCE_DIR}/include)  
-  endif()
-  target_link_libraries(flightgym PRIVATE ${LIBRARY_NAME} )
-endif()
+# if(FLIGHTLIB_GYM_SOURCES)
+#   # flightlib_gym (python3 binding with Pybind11)
+#   pybind11_add_module(flightgym MODULE 
+#     ${FLIGHTLIB_GYM_SOURCES})
+#   if(EIGEN3_FOUND)
+#     target_include_directories(flightgym PRIVATE
+#       # ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
+#       ${PYBIND11_INCLUDE_DIR}
+#       ${PROJECT_SOURCE_DIR}/include
+#       ${EIGEN3_INCLUDE_DIR})  
+#   else()
+#     target_include_directories(flightgym PRIVATE
+#       # ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include
+#       ${PYBIND11_INCLUDE_DIR}
+#       ${PROJECT_SOURCE_DIR}/externals/eigen/eigen3 #pybind11 use #include <Eigen/Core>, however, flightmare use #include <eigen3/Eigen/Core>
+#       ${PROJECT_SOURCE_DIR}/include)  
+#   endif()
+#   target_link_libraries(flightgym PRIVATE ${LIBRARY_NAME} )
+# endif()
 
 if(ENABLE_BLAS AND BLAS_FOUND AND LAPACK_FOUND)
   message(STATUS "Linking standard BLAS ${BLAS_LIBRARIES}")
@@ -241,14 +241,14 @@ if(ENABLE_BLAS AND BLAS_FOUND AND LAPACK_FOUND)
 endif()
 
 # Build tests for flightlib gym wrapper
-if(BUILD_TESTS AND FLIGHTLIB_GYM_TEST_SOURCES)
-  add_executable(test_gym ${FLIGHTLIB_GYM_TEST_SOURCES})
-  target_link_libraries(test_gym
-    ${LIBRARY_NAME}
-    gtest
-    gtest_main)
-add_test(test_gym test_gym)
-endif()
+# if(BUILD_TESTS AND FLIGHTLIB_GYM_TEST_SOURCES)
+#   add_executable(test_gym ${FLIGHTLIB_GYM_TEST_SOURCES})
+#   target_link_libraries(test_gym
+#     ${LIBRARY_NAME}
+#     gtest
+#     gtest_main)
+# add_test(test_gym test_gym)
+# endif()
 
 # Build tests for flightlib
 if(BUILD_TESTS AND FLIGHTLIB_TEST_SOURCES)

flightlib/include/flightlib/common/types.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
-#include <eigen3/Eigen/Eigen>
+#include <Eigen/Core>
+#include <Eigen/Eigen>
 
 namespace flightlib {
 

flightlib/include/flightlib/sensors/rgb_camera.hpp

@@ -45,6 +45,7 @@ class RGBCamera : SensorBase {
   bool getDepthMap(cv::Mat& depth_map);
   bool getSegmentation(cv::Mat& segmentation);
   bool getOpticalFlow(cv::Mat& opticalflow);
+  void getCameraIntrinsics(double& fx, double& fy, double& cx, double& cy);
 
   // auxiliary functions
   void enableDepth(const bool on);

flightlib/src/common/math.cpp

@@ -145,53 +145,55 @@ void quaternionToEuler(const Quaternion& quat, Ref<Vector<3>> euler) {
 std::vector<Scalar> transformationRos2Unity(const Matrix<4, 4>& ros_tran_mat) {
   /// [ Transformation Matrix ] from ROS coordinate system (right hand)
   /// to Unity coordinate system (left hand)
+  // https://github.com/siemens/ros-sharp/wiki/Dev_ROSUnityCoordinateSystemConversion
+  // https://github.com/RobotecAI/ros2-for-unity/blob/develop/src/Ros2ForUnity/Scripts/Transformations.cs
   Matrix<4, 4> tran_mat = Matrix<4, 4>::Zero();
-  tran_mat(0, 0) = 1.0;
-  tran_mat(1, 2) = 1.0;
+  tran_mat(0, 2) = 1.0;
+  tran_mat(1, 0) = -1.0;
   tran_mat(2, 1) = 1.0;
   tran_mat(3, 3) = 1.0;
-  //
+
   Matrix<4, 4> unity_tran_mat = tran_mat * ros_tran_mat * tran_mat.transpose();
-  // std::vector<Scalar> unity_tran_mat_vec(
-  //   unity_tran_mat.data(),
-  //   unity_tran_mat.data() + unity_tran_mat.rows() * unity_tran_mat.cols());
+
+  std::vector<Scalar> unity_tran_mat_vec(
+    unity_tran_mat.data(),
+    unity_tran_mat.data() + unity_tran_mat.rows() * unity_tran_mat.cols());
   std::vector<Scalar> tran_unity;
   for (int i = 0; i < 4; ++i) {
     for (int j = 0; j < 4; ++j) {
       tran_unity.push_back(unity_tran_mat(i, j));
     }
   }
+
   return tran_unity;
 }
 
 std::vector<Scalar> quaternionRos2Unity(const Quaternion& ros_quat) {
   /// [ Quaternion ] from ROS coordinate system (right hand)
   /// to Unity coordinate system (left hand)
-  Matrix<3, 3> rot_mat = Matrix<3, 3>::Zero();
-  rot_mat(0, 0) = 1.0;
-  rot_mat(1, 2) = 1.0;
-  rot_mat(2, 1) = 1.0;
-  //
-  Matrix<3, 3> unity_rot_mat =
-    rot_mat * ros_quat.toRotationMatrix() * rot_mat.transpose();
-  Quaternion unity_quat(unity_rot_mat);
-  std::vector<Scalar> unity_quat_vec{unity_quat.x(), unity_quat.y(),
-                                     unity_quat.z(), unity_quat.w()};
+  // https://github.com/siemens/ros-sharp/wiki/Dev_ROSUnityCoordinateSystemConversion
+  // https://github.com/RobotecAI/ros2-for-unity/blob/develop/src/Ros2ForUnity/Scripts/Transformations.cs
+  std::vector<Scalar> unity_quat_vec{ros_quat.y(), -ros_quat.z(),
+                                     -ros_quat.x(), ros_quat.w()};
   return unity_quat_vec;
 }
 
 std::vector<Scalar> positionRos2Unity(const Vector<3>& ros_pos_vec) {
   /// [ Position Vector ] from ROS coordinate system (right hand)
   /// to Unity coordinate system (left hand)
-  std::vector<Scalar> unity_position{ros_pos_vec(0), ros_pos_vec(2),
-                                     ros_pos_vec(1)};
+  // https://github.com/siemens/ros-sharp/wiki/Dev_ROSUnityCoordinateSystemConversion
+  // https://github.com/RobotecAI/ros2-for-unity/blob/develop/src/Ros2ForUnity/Scripts/Transformations.cs
+  std::vector<Scalar> unity_position{-ros_pos_vec(1), ros_pos_vec(2),
+                                     ros_pos_vec(0)};
   return unity_position;
 }
 
 std::vector<Scalar> scalarRos2Unity(const Vector<3>& ros_scalar) {
   /// [ Object Scalar Vector ] from ROS coordinate system (right hand)
   /// to Unity coordinate system (left hand)
-  std::vector<Scalar> unity_scalar{ros_scalar(0), ros_scalar(2), ros_scalar(1)};
+  // https://github.com/siemens/ros-sharp/wiki/Dev_ROSUnityCoordinateSystemConversion
+  // https://github.com/RobotecAI/ros2-for-unity/blob/develop/src/Ros2ForUnity/Scripts/Transformations.cs
+  std::vector<Scalar> unity_scalar{-ros_scalar(1), ros_scalar(2), ros_scalar(0)};
   return unity_scalar;
 }
 

flightlib/src/sensors/rgb_camera.cpp

@@ -181,4 +181,16 @@ bool RGBCamera::getOpticalFlow(cv::Mat& opticalflow) {
   return false;
 }
 
+void RGBCamera::getCameraIntrinsics(double& fx, double& fy, double& cx, double& cy) {
+  // fov_ is the vertical fov in degrees
+  double fovy_radians = (M_PI/180.0) * fov_;
+  double fovx_radians = 2.0 * std::atan(std::tan(fovy_radians / 2) * width_ / height_);
+  double tan_invx = 1.0 / tan(0.5 * fovx_radians);
+  double tan_invy = 1.0 / tan(0.5 * fovy_radians);
+  cx = width_ / 2.0;
+  cy = height_ / 2.0;
+  fx = cx * tan_invx;
+  fy = cy * tan_invy;
+}
+
 }  // namespace flightlib

flightros/CMakeLists.txt

@@ -38,25 +38,25 @@ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -Wall -g")
 
 #pilot
 
-cs_add_library(flight_pilot
-    src/pilot/flight_pilot.cpp
-)
-
-target_link_libraries(flight_pilot
-  ${catkin_LIBRARIES}
-  ${OpenCV_LIBRARIES}
-  stdc++fs
-)
-
-cs_add_executable(flight_pilot_node
-   src/pilot/flight_pilot_node.cpp
-)
-
-target_link_libraries(flight_pilot_node
-  flight_pilot
-  ${OpenCV_LIBRARIES}
-  stdc++fs
-)
+# cs_add_library(flight_pilot
+#     src/pilot/flight_pilot.cpp
+# )
+
+# target_link_libraries(flight_pilot
+#   ${catkin_LIBRARIES}
+#   ${OpenCV_LIBRARIES}
+#   stdc++fs
+# )
+
+# cs_add_executable(flight_pilot_node
+#    src/pilot/flight_pilot_node.cpp
+# )
+
+# target_link_libraries(flight_pilot_node
+#   flight_pilot
+#   ${OpenCV_LIBRARIES}
+#   stdc++fs
+# )
 
 # motion_planning
 
@@ -124,17 +124,17 @@ LIBRARIES
 CATKIN_DEPENDS
 )
 
-cs_add_executable(racing
-    src/racing/racing.cpp
-)
-
-target_link_libraries(racing
-${catkin_LIBRARIES}
-${OpenCV_LIBRARIES}
-stdc++fs
-zmq
-zmqpp
-)
+# cs_add_executable(racing
+#     src/racing/racing.cpp
+# )
+
+# target_link_libraries(racing
+# ${catkin_LIBRARIES}
+# ${OpenCV_LIBRARIES}
+# stdc++fs
+# zmq
+# zmqpp
+# )
 
 # camera
 

flightros/launch/pilot/rotors_gazebo.launch

@@ -113,10 +113,6 @@
       <rosparam file="$(find rpg_rotors_interface)/parameters/manual_flight_assistant.yaml"/>
     </node>
 
-    <node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
-        args="-s rqt_quad_gui.basic_flight.BasicFlight --args
-        --quad_name $(arg quad_name)" output="screen"/>
-
   </group>
 
 </launch>

flightros/package.xml

@@ -13,19 +13,14 @@
 
   <depend>flightlib</depend>
   <depend>roscpp</depend>
-  <depend>autopilot</depend>
-  <depend>quadrotor_common</depend>
   <depend>nav_msgs</depend>
   <depend>eigen_catkin</depend>
-  <depend>quadrotor_msgs</depend>
-  <depend>polynomial_trajectories</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>tf</depend>
   <depend>tf_conversions</depend>
   <depend>eigen_conversions</depend>
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
-  <depend>mav_msgs</depend>
 
 </package>

flightros/src/camera/camera.cpp

@@ -39,15 +39,15 @@ int main(int argc, char *argv[]) {
 
   // Flightmare(Unity3D)
   std::shared_ptr<UnityBridge> unity_bridge_ptr = UnityBridge::getInstance();
-  SceneID scene_id{UnityScene::WAREHOUSE};
+  SceneID scene_id{UnityScene::NATUREFOREST};
   bool unity_ready{false};
 
   // initialize publishers
   image_transport::ImageTransport it(pnh);
   rgb_pub = it.advertise("/rgb", 1);
   depth_pub = it.advertise("/depth", 1);
-  segmentation_pub = it.advertise("/segmentation", 1);
-  opticalflow_pub = it.advertise("/opticalflow", 1);
+  // segmentation_pub = it.advertise("/segmentation", 1);
+  // opticalflow_pub = it.advertise("/opticalflow", 1);
 
   // Flightmare
   Vector<3> B_r_BC(0.0, 0.0, 0.3);
@@ -63,6 +63,12 @@ int main(int argc, char *argv[]) {
 
   // initialization
   quad_state.setZero();
+  if(scene_id == UnityScene::NATUREFOREST)
+  {
+    quad_state.x[QS::POSX] = 101;
+    quad_state.x[QS::POSY] = 86;
+    quad_state.x[QS::POSZ] = 31;
+  }
   quad_ptr->reset(quad_state);
 
   // connect unity