merge dev/ros

.gitignore

@@ -14,14 +14,8 @@ __pycache__
 !.vscode/extensions.json
 
 # General
-build/
 _build/
 .sass-cache
 _posts
 _site
-externals/*
 
-#Flightmare
-flightlib
-flightrl
-flighros

flightlib/build/.gitignore

@@ -3,4 +3,4 @@
 # Except this file
 !.gitignore
 !.clang-format
-!setup.py
+!setup.py

flightlib/build/setup.py

@@ -0,0 +1,14 @@
+from setuptools import setup, Extension
+#
+setup(name='flightgym',
+      version='0.0.1',
+      author="Yunlong Song",
+      author_email='[email protected]',
+      description="Flightmare: A Quadrotor Simulator",
+      long_description='',
+      packages=[''],
+      package_dir={'': './'},
+      package_data={'': ['flightgym.cpython-36m-x86_64-linux-gnu.so']},
+      zip_fase=True,
+      url=None,
+      )

flightlib/include/flightlib/bridges/unity_bridge.hpp

@@ -96,7 +96,7 @@ class UnityBridge {
   int64_t u_packet_latency_;
 
   // axuiliary variables
-  const Scalar unity_connection_time_out_{10.0};
+  const Scalar unity_connection_time_out_{60.0};
   bool unity_ready_{false};
 };
 }  // namespace flightlib

flightlib/include/flightlib/objects/quadrotor.hpp

@@ -41,6 +41,7 @@ class Quadrotor : ObjectBase {
   Quaternion getQuaternion(void) const;
   std::vector<std::shared_ptr<RGBCamera>> getCameras(void) const;
   bool getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const;
+  bool getCollision() const;
 
   // public set functions
   bool setState(const QuadState& state);

flightlib/setup.py

@@ -33,7 +33,7 @@ def run(self):
         FLIGHTLIB_BUILD_FILES = os.environ["FLIGHTMARE_PATH"] + \
             "/flightlib/build/"
         # --------------------------------
-        # remove cached build files
+        # remove cached files
         # a hack to solve some cmake error when using "pip install ."
         try:
             for i, p in enumerate(glob.glob(os.path.join(FLIGHTLIB_BUILD_FILES, "*"))):
@@ -104,4 +104,4 @@ def build_extension(self, ext):
     cmdclass=dict(build_ext=CMakeBuild),
     include_package_data=True,
     zip_safe=False,
-) 
+)

flightlib/src/bridges/unity_bridge.cpp

@@ -194,6 +194,7 @@ bool UnityBridge::addStaticObject(std::shared_ptr<StaticObject> static_object) {
   static_objects_.push_back(static_object);
   settings_.objects.push_back(object_t);
   pub_msg_.objects.push_back(object_t);
+  //
   return true;
 }
 

flightlib/src/objects/quadrotor.cpp

@@ -258,4 +258,6 @@ bool Quadrotor::getCamera(const size_t cam_id,
   return true;
 }
 
+bool Quadrotor::getCollision() const { return collision_; }
+
 }  // namespace flightlib

flightrl/setup.py

@@ -15,7 +15,7 @@
     author_email='[email protected]',
     description='Flightmare: A Quadrotor Simulator.',
     long_description='',
-    install_requires=[''],
-    packages=[package for package in find_packages()
-              if package.startswith('rpg_baselines')]
+    install_requires=['gym==0.11', 'ruamel.yaml',
+                      'numpy', 'stable_baselines==2.10.1'],
+    packages=['rpg_baselines'],
 )

flightros/include/flightros/motion_planning/motion_planning.hpp

@@ -35,75 +35,47 @@
 #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};
+struct Bounds {
+  float3 min;
+  float3 max;
+};
 
+class MotionPlanner {
+ public:
+  MotionPlanner();
+  ~MotionPlanner();
+  void run();
+  void readPointCloud();
+  void getBounds();
+  bool plan();
+  void executePath();
+
+ private:
+  std::vector<ompl::base::State *> path_;
+  std::vector<Eigen::Vector3d> vecs_;
+  std::vector<float3> verts_;
+  open3d::geometry::KDTreeFlann kd_tree_;
+  Eigen::MatrixXd points_;
+  Bounds bounds_;
+
+  // unity
+  SceneID scene_id_{UnityScene::NATUREFOREST};
+
+  // ompl methods
+  bool searchRadius(const Eigen::Vector3d &query_point, const double radius);
+  Eigen::Vector3d stateToEigen(const ompl::base::State *state);
+  bool isStateValid(const ob::State *state);
+};
 
 }  // namespace motion_planning

flightros/include/flightros/motion_planning/tinyply.h

@@ -37,6 +37,7 @@
 
 namespace tinyply {
 
+
 enum class Type : uint8_t {
   INVALID,
   INT8,

flightros/launch/camera/camera.launch

@@ -1,6 +1,6 @@
 <launch>
     <arg name="debug" default="0" />
-     <arg name="use_unity_editor" default="false" />
+    <arg name="use_unity_editor" default="false" />
 
     <!-- RPG Flightmare Unity Render. -->
     <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">

flightros/launch/camera/camera.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
       Splitter Ratio: 0.5
-    Tree Height: 184
+    Tree Height: 190
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -88,6 +88,18 @@ Visualization Manager:
       Transport Hint: raw
       Unreliable: false
       Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /opticalflow
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -139,12 +151,12 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1124
+  Height: 1130
   Hide Left Dock: false
   Hide Right Dock: true
   Image:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -153,6 +165,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2048
+  Width: 1920
   X: 0
-  Y: 74
+  Y: 0

flightros/launch/motion_planning/motion_planning.launch

@@ -1,6 +1,11 @@
 <launch>
     <arg name="debug" default="0" />
-
+    <arg name="use_unity_editor" default="false" />
+
+    <!-- RPG Flightmare Unity Render. -->
+    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">
+    </node>
+
     <node name="motion_planning" pkg="flightros" type="motion_planning" output="screen" launch-prefix="gdb -ex run --args" if="$(arg debug)" >
     </node>
     <node name="motion_planning" pkg="flightros" type="motion_planning" output="screen" unless="$(arg debug)">

flightros/launch/pilot/rotors_gazebo_test.launch

@@ -1,11 +1,14 @@
 <?xml version="1.0"?>
 <launch>
+    <arg name="use_unity_editor" default="false" />
+
     <node pkg="flightros" type="flight_pilot_node" name="flight_pilot_node" output="screen">
       <rosparam file="$(find flightros)/params/default.yaml" /> 
       <remap from="flight_pilot/state_estimate" to="ground_truth/odometry" />
     </node>
 
-    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render">
+    <!-- RPG Flightmare Unity Render. -->
+    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">
     </node>
 
 </launch>

flightros/launch/racing/racing.launch

@@ -1,9 +1,10 @@
 <?xml version="1.0"?>
 <launch>
     <arg name="debug" default="0" />
-
+    <arg name="use_unity_editor" default="false" />
+
     <!-- RPG Flightmare Unity Render. -->
-    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" >
+    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">
     </node>
 
     <node name="racing" pkg="flightros" type="racing" output="screen" launch-prefix="gdb -ex run --args" if="$(arg debug)" >

flightros/package.xml

@@ -26,5 +26,6 @@
   <depend>eigen_conversions</depend>
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
+  <depend>mav_msgs</depend>
 
 </package>

flightros/src/camera/camera.cpp

@@ -100,13 +100,11 @@ int main(int argc, char *argv[]) {
     segmentation_msg->header.stamp = timestamp;
     segmentation_pub.publish(segmentation_msg);
 
-    // // The current optical flow is not correct.
-    // // you can still visualize it by uncomment the following code.
-    // rgb_camera->getOpticalFlow(img);
-    // sensor_msgs::ImagePtr opticflow_msg =
-    //   cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
-    // opticflow_msg->header.stamp = timestamp;
-    // opticalflow_pub.publish(opticflow_msg);
+    rgb_camera->getOpticalFlow(img);
+    sensor_msgs::ImagePtr opticflow_msg =
+      cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
+    opticflow_msg->header.stamp = timestamp;
+    opticalflow_pub.publish(opticflow_msg);
 
     frame_id += 1;
   }