forked from uzh-rpg/flightmare
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request uzh-rpg#42 from uzh-rpg/dev/samples
Dev/samples
- Loading branch information
Showing
28 changed files
with
23,781 additions
and
18 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
109 changes: 109 additions & 0 deletions
109
flightros/include/flightros/motion_planning/motion_planning.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,109 @@ | ||
| // standard libraries | ||
| #include <assert.h> | ||
| #include <Eigen/Dense> | ||
| #include <cmath> | ||
| #include <cstring> | ||
| #include <experimental/filesystem> | ||
| #include <fstream> | ||
| #include <iostream> | ||
| #include <iterator> | ||
| #include <sstream> | ||
| #include <thread> | ||
| #include <vector> | ||
|
|
||
| // Open3D | ||
| #include <Open3D/Geometry/KDTreeFlann.h> | ||
| #include <Open3D/Geometry/PointCloud.h> | ||
| #include <Open3D/IO/ClassIO/PointCloudIO.h> | ||
|
|
||
| // OMPL | ||
| #include <ompl/base/SpaceInformation.h> | ||
| #include <ompl/base/spaces/SE3StateSpace.h> | ||
| #include <ompl/config.h> | ||
| #include <ompl/geometric/SimpleSetup.h> | ||
| #include <ompl/geometric/planners/rrt/RRTConnect.h> | ||
|
|
||
| // TinyPly | ||
| #define TINYPLY_IMPLEMENTATION | ||
| #include "tinyply.h" | ||
|
|
||
| // 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/sensors/rgb_camera.hpp" | ||
|
|
||
| // ros | ||
| #include <ros/ros.h> | ||
|
|
||
| namespace ob = ompl::base; | ||
| namespace og = ompl::geometric; | ||
|
|
||
| using namespace flightlib; | ||
|
|
||
| namespace motion_planning { | ||
|
|
||
| struct float2 { | ||
| float x, y; | ||
| }; | ||
| struct float3 { | ||
| float x, y, z; | ||
| }; | ||
| struct double3 { | ||
| double x, y, z; | ||
| }; | ||
| struct uint3 { | ||
| uint32_t x, y, z; | ||
| }; | ||
| struct uint4 { | ||
| uint32_t x, y, z, w; | ||
| }; | ||
| std::vector<float3> verts; | ||
| float range = 1; | ||
| bool solution_found = false; | ||
| bool trajectory_found = false; | ||
|
|
||
| std::vector<float3> readPointCloud(); | ||
| float3 min_bounds; | ||
| float3 max_bounds; | ||
|
|
||
| Eigen::Vector3d stateToEigen(const ompl::base::State *state); | ||
|
|
||
| std::vector<ompl::base::State *> path_; | ||
| std::vector<Eigen::Vector3d> vecs_; | ||
|
|
||
| void getBounds(); | ||
|
|
||
| void plan(); | ||
|
|
||
| bool isStateValid(const ob::State *state); | ||
|
|
||
| bool isInRange(float x, float y, float z); | ||
|
|
||
| open3d::geometry::KDTreeFlann kd_tree_; | ||
| Eigen::MatrixXd points_; | ||
| bool searchRadius(const Eigen::Vector3d &query_point, const double radius); | ||
|
|
||
| void executePath(); | ||
|
|
||
| // void setupQuad(); | ||
| bool setUnity(const bool render); | ||
| bool connectUnity(void); | ||
|
|
||
| // unity quadrotor | ||
| std::shared_ptr<Quadrotor> quad_ptr_; | ||
| std::shared_ptr<RGBCamera> rgb_camera_; | ||
| QuadState quad_state_; | ||
|
|
||
| // Flightmare(Unity3D) | ||
| std::shared_ptr<UnityBridge> unity_bridge_ptr_; | ||
| SceneID scene_id_{UnityScene::NATUREFOREST}; | ||
| bool unity_ready_{false}; | ||
| bool unity_render_{true}; | ||
| RenderMessage_t unity_output_; | ||
| uint16_t receive_id_{0}; | ||
|
|
||
|
|
||
| } // namespace motion_planning |
Oops, something went wrong.