finish px4 avoidance

README.md

@@ -31,7 +31,7 @@ This repo is only tested in Ubuntu18.04 with ROS Melodic.
    2. Note: The local-planner in PX4-avoidance and fast-planner using different config file. Please select the right config file when you start simulation using `roslaunch px4_avoidance_airsim start_simulation.launch`.
 3. Launch planning algorithms
 
-   1. Such as `roslaunch px4_avoidance_airsim start_fast_planner.launch`
+   1. Such as `roslaunch px4_avoidance_airsim start_fast_planner.launch` or `roslaunch px4_avoidance_airsim start_fast_planner.launch`
 4. Have fun
 
 ## Speed up the mavros topics
@@ -54,3 +54,23 @@ MC_PITCHRATE_MAX 60 deg/s
 MC_ROLLRATE_MAX 60 deg/s
 
 MC_YAWRATE_MAX 60 deg/s
+
+## Change Fast-planner goal height
+
+method 1: 
+
+in `fast_planner/plan_manage/src/kino_replan_fsm.cpp`
+
+```
+if (target_type_ == TARGET_TYPE::MANUAL_TARGET) {  
+	end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, 1.0;
+}
+```
+
+method 2:
+
+in `uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp` line 146
+
+```
+voidgoal_callback(constgeometry_msgs::PoseStamped::ConstPtr&msg) 
+```

launch/start_local_planner.launch → launch/start_px4_avoidance.launch

@@ -25,7 +25,7 @@
     <arg name="manager"             default="local_planner_manager"/>
 
     <!-- rename local planner output before create nodelet -->
-    <remap from="/mavros/setpoint_position/local" to="/input_setpoint_position_local" />
+    <!-- <remap from="/mavros/setpoint_position/local" to="/input_setpoint_position_local" /> -->
     <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
 
     <node pkg="nodelet" type="nodelet" name="local_planner_nodelet" args="load LocalPlannerNodelet $(arg manager)" output="screen">

rviz/local_planner.rviz

@@ -5,10 +5,9 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Image1
-        - /PointCloud21
+        - /histogram1
       Splitter Ratio: 0.5
-    Tree Height: 381
+    Tree Height: 140
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -273,6 +272,42 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /airsim_node/drone_1/camera_1/Scene
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: rgb camera
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /cost_image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: cost image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /histogram_image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: histogram
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 89; 120; 76
@@ -296,7 +331,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 42.6451416015625
+      Distance: 24.84286117553711
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -311,10 +346,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.1297966241836548
+      Pitch: 0.9647964239120483
       Target Frame: fcu
       Value: ThirdPersonFollower (rviz)
-      Yaw: 3.184230089187622
+      Yaw: 2.7442286014556885
     Saved:
       - Class: rviz/ThirdPersonFollower
         Distance: 10
@@ -359,12 +394,12 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1050
+  Height: 1023
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000370fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000a000000039500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ba000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001fd000001b00000001600fffffffb0000000a0049006d00610067006500000002ce000000c000000000000000000000000100000132000004f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000004f2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000004afc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003830000037000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000355fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000a000000039500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000000c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000010c0000008b0000001600fffffffb0000000a0049006d00610067006500000002ce000000c00000000000000000fb00000014007200670062002000630061006d006500720061010000019d000000960000001600fffffffb000000140063006f0073007400200069006d0061006700650100000239000000af0000001600fffffffb000000120068006900730074006f006700720061006d01000002ee000000a40000001600ffffff0000000100000132000004f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000004f2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000004afc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003830000035500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -375,4 +410,10 @@ Window Geometry:
     collapsed: false
   Width: 1267
   X: 653
-  Y: 0
+  Y: 27
+  cost image:
+    collapsed: false
+  histogram:
+    collapsed: false
+  rgb camera:
+    collapsed: false