From 204b3d64b0b6d6c47eae3e4ca53809807e72221e Mon Sep 17 00:00:00 2001 From: slimeth Date: Tue, 17 Nov 2020 16:14:18 +0100 Subject: [PATCH] pull request fixes --- .gitignore | 1 + README.md | 10 +- .../flightlib/bridges/unity_bridge.hpp | 1 - flightros/CMakeLists.txt | 113 +++++++++--------- flightros/include/flightros/camera/camera.hpp | 3 +- .../motion_planning/motion_planning.hpp | 18 --- flightros/include/flightros/racing/racing.hpp | 17 --- flightros/src/camera/camera.cpp | 12 +- .../src/motion_planning/motion_planning.cpp | 46 +------ flightros/src/racing/racing.cpp | 7 +- 10 files changed, 71 insertions(+), 157 deletions(-) diff --git a/.gitignore b/.gitignore index cea4d897ca..3de62736f0 100644 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,4 @@ dist *.so __pycache__ build +_build diff --git a/README.md b/README.md index b732272d8a..76dd7213e4 100644 --- a/README.md +++ b/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} } ``` diff --git a/flightlib/include/flightlib/bridges/unity_bridge.hpp b/flightlib/include/flightlib/bridges/unity_bridge.hpp index 0870036bc7..1434a49985 100644 --- a/flightlib/include/flightlib/bridges/unity_bridge.hpp +++ b/flightlib/include/flightlib/bridges/unity_bridge.hpp @@ -6,7 +6,6 @@ // std libs #include -#include #include #include #include diff --git a/flightros/CMakeLists.txt b/flightros/CMakeLists.txt index ffc51cdf83..e9289266be 100644 --- a/flightros/CMakeLists.txt +++ b/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,11 +68,15 @@ 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 () @@ -87,69 +84,69 @@ if(BUILD_SAMPLES) 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() diff --git a/flightros/include/flightros/camera/camera.hpp b/flightros/include/flightros/camera/camera.hpp index fc59881ba7..cfc7fd4b80 100644 --- a/flightros/include/flightros/camera/camera.hpp +++ b/flightros/include/flightros/camera/camera.hpp @@ -1,4 +1,3 @@ -#pragma once // ros #include #include @@ -7,6 +6,7 @@ // standard libraries #include #include +#include #include #include #include @@ -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); diff --git a/flightros/include/flightros/motion_planning/motion_planning.hpp b/flightros/include/flightros/motion_planning/motion_planning.hpp index 07ad15a5db..1ad557dce8 100644 --- a/flightros/include/flightros/motion_planning/motion_planning.hpp +++ b/flightros/include/flightros/motion_planning/motion_planning.hpp @@ -1,9 +1,6 @@ -#pragma once - // standard libraries #include #include -#include #include #include #include @@ -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( - std::chrono::high_resolution_clock::now() - t0) - .count() * - 1000.0; - } - const double &get() { return timestamp; } -}; - struct float2 { float x, y; }; diff --git a/flightros/include/flightros/racing/racing.hpp b/flightros/include/flightros/racing/racing.hpp index 46bd1f53a6..09ee78ec3f 100644 --- a/flightros/include/flightros/racing/racing.hpp +++ b/flightros/include/flightros/racing/racing.hpp @@ -1,5 +1,3 @@ -#pragma once - // ros #include #include @@ -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( - 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); diff --git a/flightros/src/camera/camera.cpp b/flightros/src/camera/camera.cpp index 77f31e3539..b1864a8bf1 100644 --- a/flightros/src/camera/camera.cpp +++ b/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; diff --git a/flightros/src/motion_planning/motion_planning.cpp b/flightros/src/motion_planning/motion_planning.cpp index 83fdc5004a..6debc559ad 100644 --- a/flightros/src/motion_planning/motion_planning.cpp +++ b/flightros/src/motion_planning/motion_planning.cpp @@ -64,16 +64,7 @@ std::vector motion_planning::readPointCloud() { std::cerr << "tinyply exception: " << e.what() << std::endl; } - manual_timer read_timer; - - read_timer.start(); file.read(*file_stream); - read_timer.stop(); - - const float parsing_time = read_timer.get() / 1000.f; - std::cout << "\tparsing " << size_mb << "mb in " << parsing_time - << " seconds [" << (size_mb / parsing_time) << " MBps]" - << std::endl; if (vertices) std::cout << "\tRead " << vertices->count << " total vertices " @@ -136,7 +127,6 @@ void motion_planning::getBounds() { } } - void motion_planning::plan() { // construct the state space we are planning in auto space(std::make_shared()); @@ -259,9 +249,6 @@ Eigen::Vector3d motion_planning::stateToEigen(const ompl::base::State *state) { float y = pos->values[1]; float z = pos->values[2]; - // return a value that is always true but uses the two variables we define, so - // we avoid compiler warnings - // return isInRange(x, y, z); Eigen::Vector3d query_pos{x, y, z}; return query_pos; } @@ -280,40 +267,11 @@ bool motion_planning::isStateValid(const ob::State *state) { float x = pos->values[0]; float y = pos->values[1]; float z = pos->values[2]; - // return a value that is always true but uses the two variables we define, so - // we avoid compiler warnings - // return isInRange(x, y, z); + Eigen::Vector3d query_pos{x, y, z}; return searchRadius(query_pos, range); } -bool motion_planning::isInRange(float x, float y, float z) { - bool validState = true; - bool outOfBound = false; - - for (const auto &ver : verts) { - // check if box around quad is occupied - if (abs(ver.z - z) <= range) { - if (abs(ver.x - x) <= range) { - if (abs(ver.y - y) <= range) { - validState = false; - } - } - } else { - if (ver.z - z >= 0) { - outOfBound = true; - } - } - - - if (outOfBound || !validState) { - break; - } - } - return validState; -} - - bool motion_planning::searchRadius(const Eigen::Vector3d &query_point, const double radius) { std::vector indices; @@ -403,7 +361,6 @@ void motion_planning::executePath() { } } - bool motion_planning::setUnity(const bool render) { unity_render_ = render; if (unity_render_ && unity_bridge_ptr_ == nullptr) { @@ -421,7 +378,6 @@ bool motion_planning::connectUnity() { return unity_ready_; } - int main(int argc, char *argv[]) { // initialize ROS ros::init(argc, argv, "flightmare_example"); diff --git a/flightros/src/racing/racing.cpp b/flightros/src/racing/racing.cpp index 57ef125787..179cd8cd6d 100644 --- a/flightros/src/racing/racing.cpp +++ b/flightros/src/racing/racing.cpp @@ -95,15 +95,12 @@ int main(int argc, char *argv[]) { 20.0, 20.0, 6.0); // Start racing - racing::manual_timer timer; - timer.start(); + ros::Time t0 = ros::Time::now(); while (ros::ok() && racing::unity_render_ && racing::unity_ready_) { - timer.stop(); - quadrotor_common::TrajectoryPoint desired_pose = polynomial_trajectories::getPointFromTrajectory( - trajectory, ros::Duration(timer.get() / 1000)); + trajectory, ros::Duration((ros::Time::now() - t0))); racing::quad_state_.x[QS::POSX] = (Scalar)desired_pose.position.x(); racing::quad_state_.x[QS::POSY] = (Scalar)desired_pose.position.y(); racing::quad_state_.x[QS::POSZ] = (Scalar)desired_pose.position.z();