diff --git a/README.md b/README.md index 46fb99524d..961bb05357 100644 --- a/README.md +++ b/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}, diff --git a/docs/source/conf.py b/docs/source/conf.py index cdcdb5887e..660ad59819 100644 --- a/docs/source/conf.py +++ b/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 +# ], +# } diff --git a/flightlib/CMakeLists.txt b/flightlib/CMakeLists.txt index 3ec7e11632..0c182ef146 100644 --- a/flightlib/CMakeLists.txt +++ b/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 , however, flightmare use #include - ${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 , however, flightmare use #include +# ${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) diff --git a/flightlib/include/flightlib/common/types.hpp b/flightlib/include/flightlib/common/types.hpp index 966b0b617e..6bb7f84d8e 100644 --- a/flightlib/include/flightlib/common/types.hpp +++ b/flightlib/include/flightlib/common/types.hpp @@ -1,6 +1,7 @@ #pragma once -#include +#include +#include namespace flightlib { diff --git a/flightlib/include/flightlib/sensors/rgb_camera.hpp b/flightlib/include/flightlib/sensors/rgb_camera.hpp index 67dd2c20f7..95f553c671 100644 --- a/flightlib/include/flightlib/sensors/rgb_camera.hpp +++ b/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); diff --git a/flightlib/src/common/math.cpp b/flightlib/src/common/math.cpp index 93ba55428d..61bf002c68 100644 --- a/flightlib/src/common/math.cpp +++ b/flightlib/src/common/math.cpp @@ -145,53 +145,55 @@ void quaternionToEuler(const Quaternion& quat, Ref> euler) { std::vector 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 unity_tran_mat_vec( - // unity_tran_mat.data(), - // unity_tran_mat.data() + unity_tran_mat.rows() * unity_tran_mat.cols()); + + std::vector unity_tran_mat_vec( + unity_tran_mat.data(), + unity_tran_mat.data() + unity_tran_mat.rows() * unity_tran_mat.cols()); std::vector 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 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 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 unity_quat_vec{ros_quat.y(), -ros_quat.z(), + -ros_quat.x(), ros_quat.w()}; return unity_quat_vec; } std::vector positionRos2Unity(const Vector<3>& ros_pos_vec) { /// [ Position Vector ] from ROS coordinate system (right hand) /// to Unity coordinate system (left hand) - std::vector 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 unity_position{-ros_pos_vec(1), ros_pos_vec(2), + ros_pos_vec(0)}; return unity_position; } std::vector scalarRos2Unity(const Vector<3>& ros_scalar) { /// [ Object Scalar Vector ] from ROS coordinate system (right hand) /// to Unity coordinate system (left hand) - std::vector 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 unity_scalar{-ros_scalar(1), ros_scalar(2), ros_scalar(0)}; return unity_scalar; } diff --git a/flightlib/src/sensors/rgb_camera.cpp b/flightlib/src/sensors/rgb_camera.cpp index f3efc8752f..15a0076a5e 100644 --- a/flightlib/src/sensors/rgb_camera.cpp +++ b/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 \ No newline at end of file diff --git a/flightros/CMakeLists.txt b/flightros/CMakeLists.txt index d2e24cc4c4..cba59a5227 100644 --- a/flightros/CMakeLists.txt +++ b/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 diff --git a/flightros/launch/pilot/rotors_gazebo.launch b/flightros/launch/pilot/rotors_gazebo.launch index a422e03765..97058bafd2 100644 --- a/flightros/launch/pilot/rotors_gazebo.launch +++ b/flightros/launch/pilot/rotors_gazebo.launch @@ -113,10 +113,6 @@ - - diff --git a/flightros/package.xml b/flightros/package.xml index eaafb11757..89aa5551d8 100644 --- a/flightros/package.xml +++ b/flightros/package.xml @@ -13,12 +13,8 @@ flightlib roscpp - autopilot - quadrotor_common nav_msgs eigen_catkin - quadrotor_msgs - polynomial_trajectories std_msgs geometry_msgs tf @@ -26,6 +22,5 @@ eigen_conversions image_transport cv_bridge - mav_msgs \ No newline at end of file diff --git a/flightros/src/camera/camera.cpp b/flightros/src/camera/camera.cpp index dcef29fd22..040ae7e3e2 100644 --- a/flightros/src/camera/camera.cpp +++ b/flightros/src/camera/camera.cpp @@ -39,15 +39,15 @@ int main(int argc, char *argv[]) { // Flightmare(Unity3D) std::shared_ptr 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