improve reliability; update readme

HKUST-Aerial-Robotics · Dec 9, 2017 · a310add · a310add
1 parent 5080791
commit a310add
Show file tree

Hide file tree

Showing 41 changed files with 1,813 additions and 2,921 deletions.
diff --git a/README.md b/README.md
@@ -14,7 +14,7 @@ for Quadrotor Flight in Complex Environments,** Fei Gao, Yi Lin and Shaojie Shen
 
 Video of this paper can be found [here](http://www.bilibili.com/video/av16979476/).
 
-&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;[![](https://github.com/ZbyLGsc/trajectory_opti/blob/master/pic/video.png)](http://www.bilibili.com/video/av16979476/)
+&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;[![](https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization/blob/master/pic/video.png)](http://www.bilibili.com/video/av16979476/)
 
 
 If you use this generator for your academic research, please cite our related paper.
@@ -54,43 +54,39 @@ for Quadrotor Flight in Complex Environments},
   Clone the repository to your catkin workspace and catkin_make. For example:
 ```
   cd ~/catkin_ws/src
-  git clone https://github.com/HKUST-Aerial-Robotics/Gradient_Generator.git
+  git clone https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization.git
   cd ../
   catkin_make
   source ~/catkin_ws/devel/setup.bash
 ```
-  If you meet some errors while building, run
-
-```
-  source ~/catkin_ws/devel/setup.bash
-```
-
-  and try again.
+
 
 ## 4.Random Map and Waypoints Example
+Open two terminals and run:
 ```
-  roslaunch trajectory_optimization random.launch
+  roslaunch grad_traj_optimization traj_rviz.launch
+  roslaunch grad_traj_optimization random.launch
 ```
 
   After running and open *rviz* with traj.rviz file, you should find a randomly built collision map with some waypoints going through it.
   Then a smooth and collision free trajectory is generated. 
 
   <div align=center>
-  <img src="https://github.com/ZbyLGsc/trajectory_opti/blob/master/pic/random.gif" width = "360" height = "360">
+  <img src="https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization/blob/master/pic/random.gif" width = "360" height = "360">
   </div>
 
 ## 5.Random Map and Clicked Waypoints Example
-
+Simply run:
 ```
-  roslaunch trajectory_optimization click.launch
+  roslaunch grad_traj_optimization click.launch
 ```
 
   Likewise, a random collision map is built but with fewer obstacles. Then you can click in *rviz* using *2D Nav Goal* to add 
   some waypoints. The default waypoint number is 9 and you can change it in click.launch. Trajectory is 
   generated as long as enough waypoints are added. 
 
   <div align=center>
-  <img src="https://github.com/ZbyLGsc/trajectory_opti/blob/master/pic/click.gif" width = "360" height = "360">
+  <img src="https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization/blob/master/pic/click.gif" width = "360" height = "360">
   </div>
 
   *Note:Trajectory with too many segments or with sharp corner is difficult to optimized and is tend to fail.*

diff --git a/include/grad_traj_optimizer.h b/include/grad_traj_optimizer.h
@@ -363,8 +363,8 @@ bool GradTrajOptimizer::optimizeTrajectory(int step)
   nlopt::opt opt(nlopt::algorithm(this->algorithm), 3 * num_dp);  // x,y,z (3*n-3) x 3
   optimizer = opt;
   optimizer.set_min_objective(GradTrajOptimizer::costFunc, this);
-  optimizer.set_xtol_abs(1e-7);
-
+  // optimizer.set_xtol_abs(1e-7);
+  
   // --------------------------step specific options-----------------------------
   if(step == OPT_INITIAL_TRY)
   {

diff --git a/launch/click.launch b/launch/click.launch
@@ -1,42 +1,41 @@
 <launch>
+  <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find grad_traj_optimization)/rviz/traj.rviz" />
   <node name="traj_opti_node1" pkg="grad_traj_optimization" type="click" output="screen">
-
-  <param name="point_num" value="9" type="int"/>
-  <param name="alg" value="24" type="int"/>
-  <param name="offset" value="3.0" type="double"/>
-  <param name="retry_offset" value="0.5" type="double"/>
-  <param name="time_limit_1" value="0.06" type="double"/>
-  <param name="time_limit_2" value="0.03" type="double"/>
-  <param name="try_limit" value="0.01" type="double"/>
-  <param name="dt" value="0.2" type="double"/>
-
-  <param name="ws" value="12.0" type="double"/>
-  <param name="wc" value="1.0" type="double"/>
-
-  <!-- <param name="time" value="2.0" type="double"/> -->
-  <param name="segment_time" value="1.0" type="double"/>
-  <param name="mean_v" value="1.0" type="double"/>
-  <param name="init_time" value="0.3" type="double"/>
-
-  <param name="alpha" value="1.0" type="double"/>
-  <param name="d0" value="0.7" type="double"/>
-  <param name="r" value="0.3" type="double"/>
-
-  <param name="alphav" value="5.0" type="double"/>
-  <param name="v0" value="2.5" type="double"/>
-  <param name="rv" value="0.5" type="double"/>
-
-  <param name="alphaa" value="5.0" type="double"/>
-  <param name="a0" value="3.5" type="double"/>
-  <param name="ra" value="0.5" type="double"/>
-
-  <param name="bos" value="3.0" type="double"/>
-  <param name="vos" value="8.0" type="double"/>
-  <param name="aos" value="10.0" type="double"/>
-
-  <param name="gd_value" value="5.0" type="double"/>
-  <param name="gd_type" value="1" type="int"/>
-
+    <param name="point_num" value="9" type="int"/>
+    <param name="alg" value="24" type="int"/>
+    <param name="offset" value="3.0" type="double"/>
+    <param name="retry_offset" value="0.5" type="double"/>
+    <param name="time_limit_1" value="0.06" type="double"/>
+    <param name="time_limit_2" value="0.03" type="double"/>
+    <param name="try_limit" value="0.01" type="double"/>
+    <param name="dt" value="0.2" type="double"/>
+
+    <param name="ws" value="12.0" type="double"/>
+    <param name="wc" value="1.0" type="double"/>
+
+    <!-- <param name="time" value="2.0" type="double"/> -->
+    <param name="segment_time" value="1.0" type="double"/>
+    <param name="mean_v" value="1.0" type="double"/>
+    <param name="init_time" value="0.3" type="double"/>
+
+    <param name="alpha" value="1.0" type="double"/>
+    <param name="d0" value="0.7" type="double"/>
+    <param name="r" value="0.3" type="double"/>
+
+    <param name="alphav" value="5.0" type="double"/>
+    <param name="v0" value="2.5" type="double"/>
+    <param name="rv" value="0.5" type="double"/>
+
+    <param name="alphaa" value="5.0" type="double"/>
+    <param name="a0" value="3.5" type="double"/>
+    <param name="ra" value="0.5" type="double"/>
+
+    <param name="bos" value="3.0" type="double"/>
+    <param name="vos" value="8.0" type="double"/>
+    <param name="aos" value="10.0" type="double"/>
+
+    <param name="gd_value" value="5.0" type="double"/>
+    <param name="gd_type" value="1" type="int"/>
   </node>
 </launch>
 
diff --git a/launch/random.launch b/launch/random.launch
@@ -1,40 +1,39 @@
 <launch>
   <node name="traj_opti_node1" pkg="grad_traj_optimization" type="random" output="screen">
-  <param name="alg" value="24" type="int"/>
-  <param name="offset" value="3.0" type="double"/>
-  <param name="retry_offset" value="0.5" type="double"/>
-  <param name="time_limit_1" value="0.06" type="double"/>
-  <param name="time_limit_2" value="0.03" type="double"/>
-  <param name="try_limit" value="0.01" type="double"/>
-  <param name="dt" value="0.2" type="double"/>
-
-  <param name="ws" value="12.0" type="double"/>
-  <param name="wc" value="0.3" type="double"/>
-
-  <!-- <param name="time" value="2.0" type="double"/> -->
-  <param name="segment_time" value="1.5" type="double"/>
-  <param name="mean_v" value="1.0" type="double"/>
-  <param name="init_time" value="0.3" type="double"/>
-
-  <param name="alpha" value="1.0" type="double"/>
-  <param name="d0" value="0.7" type="double"/>
-  <param name="r" value="0.3" type="double"/>
-
-  <param name="alphav" value="5.0" type="double"/>
-  <param name="v0" value="2.5" type="double"/>
-  <param name="rv" value="0.5" type="double"/>
-
-  <param name="alphaa" value="5.0" type="double"/>
-  <param name="a0" value="3.5" type="double"/>
-  <param name="ra" value="0.5" type="double"/>
-
-  <param name="bos" value="3.0" type="double"/>
-  <param name="vos" value="8.0" type="double"/>
-  <param name="aos" value="10.0" type="double"/>
-
-  <param name="gd_value" value="5.0" type="double"/>
-  <param name="gd_type" value="1" type="int"/>
-
+    <param name="alg" value="24" type="int"/>
+    <param name="offset" value="3.0" type="double"/>
+    <param name="retry_offset" value="0.2" type="double"/>
+    <param name="time_limit_1" value="0.08" type="double"/>
+    <param name="time_limit_2" value="0.01" type="double"/>
+    <param name="try_limit" value="0.01" type="double"/>
+    <param name="dt" value="0.2" type="double"/>
+
+    <param name="ws" value="15.0" type="double"/>
+    <param name="wc" value="0.1" type="double"/>
+
+    <!-- <param name="time" value="2.0" type="double"/> -->
+    <param name="segment_time" value="1.5" type="double"/>
+    <param name="mean_v" value="1.0" type="double"/>
+    <param name="init_time" value="0.0" type="double"/>
+
+    <param name="alpha" value="1.0" type="double"/>
+    <param name="d0" value="0.5" type="double"/>
+    <param name="r" value="0.3" type="double"/>
+
+    <param name="alphav" value="5.0" type="double"/>
+    <param name="v0" value="2.5" type="double"/>
+    <param name="rv" value="0.5" type="double"/>
+
+    <param name="alphaa" value="5.0" type="double"/>
+    <param name="a0" value="3.5" type="double"/>
+    <param name="ra" value="0.5" type="double"/>
+
+    <param name="bos" value="3.0" type="double"/>
+    <param name="vos" value="8.0" type="double"/>
+    <param name="aos" value="10.0" type="double"/>
+
+    <param name="gd_value" value="5.0" type="double"/>
+    <param name="gd_type" value="1" type="int"/>
   </node>
 </launch>
 
diff --git a/launch/random_opt.launch b/launch/random_opt.launch
diff --git a/launch/traj.launch b/launch/traj.launch
diff --git a/launch/traj_opt.launch b/launch/traj_opt.launch
diff --git a/launch/traj_rviz.launch b/launch/traj_rviz.launch
@@ -0,0 +1,3 @@
+<launch>
+  <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find grad_traj_optimization)/rviz/traj.rviz" />
+</launch>
diff --git a/lib/libarc_utilities.so b/lib/libarc_utilities.so
diff --git a/lib/libsdf_tools.so b/lib/libsdf_tools.so
diff --git a/rviz/traj.rviz b/rviz/traj.rviz
@@ -18,7 +18,6 @@ Panels:
       - /2D Pose Estimate1
       - /2D Nav Goal1
       - /Publish Point1
-      - /3D Nav Goal1
     Name: Tool Properties
     Splitter Ratio: 0.588679
   - Class: rviz/Views
@@ -64,7 +63,7 @@ Visualization Manager:
       Marker Topic: trajopt/setpoint
       Name: Marker
       Namespaces:
-        "": true
+        {}
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -95,15 +94,15 @@ Visualization Manager:
       Marker Topic: sdf_tools_tutorial_visualization
       Name: Marker
       Namespaces:
-        collision_map: true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
       Enabled: true
       Marker Topic: trajopt/traj_point
       Name: Marker
       Namespaces:
-        "": true
+        {}
       Queue Size: 100
       Value: true
   Enabled: true
@@ -146,7 +145,7 @@ Visualization Manager:
       Pitch: 1.5698
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.1605
+      Yaw: 4.7105
     Saved: ~
 Window Geometry:
   Displays: