Outdoor 2D LiDAR Path Planning

• Familiarity with outdoor position-hold flight is required first.
• The following demonstration uses Windows.

Check Whether the UAV Status Is Normal

• Connect to the UAV in Connection Settings.

1. The status bar displays the current UAV status (whether the UAV flight controller is connected, the current flight mode, control status, GPS status, whether the UAV is armed, etc.).
2. The x, y, and z positions should be around 0, indicating that the positioning data is normal.
3. A red Prometheus status indicator means that the status is abnormal. The figure below shows a normal status.

Start the Function Demo

1. Click Function Scripts.
2. Click LiDAR Obstacle Avoidance.

• The onboard command is: /home/amov/p600_experiment/src/p600_experiment/scripts/lidar_ego_outdoor_s3.sh

3. A message confirming that the corresponding function demo has started appears.

1. After the function demo runs, the list of ROS nodes currently running on the onboard computer is displayed.
2. The current CPU usage and CPU temperature are displayed.

Open the Local Visualization Interface

1. Click System Settings.
2. Click Basic Settings.
3. Click Other.
4. Click Open.

1. After opening the visualization in the previous step, the interface shown below opens automatically.
2. A 3D graphic is displayed after the visualization opens.
3. This is a main display interface similar to rviz in ROS.

1. Mouse operations in this graphical window are the same as in rviz for ROS.
   • Click and hold the left mouse button in the interface, then drag to adjust the viewing angle.
   • Press and hold the mouse wheel, then drag to adjust the planar position.
   • Scroll the mouse wheel to adjust the scale.

• Adjust the top-down view and image position to make subsequent point selection easier.

Take Off

1. Move SW1 to the bottom position to arm the UAV. SW2, SW3, and SW4 should be in the top position at this time.
2. The current Prometheus control status is INIT, which corresponds to the flight controller's POSITION mode.
3. By default, the UAV begins mapping after takeoff.
   • If colored mapping data like that shown below does not appear after takeoff and only orange inflated-obstacle data is shown, reopen the visualization.

1. Move SW2 to the middle position.
2. The current Prometheus control status is RC_POS_CONTROL, which is the flight controller's OFFBOARD mode.

1. SW2 is currently in the middle position, and SW3 and SW4 are in the top position. 2. The current Prometheus control status is RC_POS_CONTROL, which is the flight controller's OFFBOARD mode. 3. Click Video Monitoring. 4. View the current position data. 5. Click and hold the left mouse button on Hover at Current Point, then move SW2 to the bottom position. The UAV hovers at its current point and enters Prometheus-controlled COMMAND_CONTROL mode. (If you move SW2 to the bottom position without clicking Hover at Current Point, the UAV enters Prometheus-controlled COMMAND_CONTROL mode, flies above the initial point, and hovers.)

1. SW2 is currently in the bottom position, and SW3 and SW4 are in the top position.
2. The current Prometheus control status is COMMAND_CONTROL, which is also the flight controller's OFFBOARD mode.
3. The current hover position is displayed.
4. Click rviz to enter the visualization interface.

1. SW2 is currently in the bottom position, and SW3 and SW4 are in the top position.
2. The current Prometheus control status is displayed.
3. The UAV hovers at its current position and waits for a target point to be published.

Run the LiDAR Mapping and EGO Autonomous Planning and Obstacle-Avoidance Function

1. Right-click the target point.
2. The target point's X position is loaded.
3. The target point's Y position is loaded.
4. The default target altitude is 1.2 m.

1. Double-click the target position to modify the target point.
2. After confirming the target position, click Send.

• The system begins automatically planning an obstacle-avoiding path based on the target point.

1. The UAV reaches the target point.
2. Right-click to select another target position, and then send it again.
3. Noise in the LiDAR data is shown.

1. Select the target-point position.
2. After confirming the target position, click Send.
3. The system automatically plans an obstacle-avoiding path and begins flying.

Land the UAV

Land Using Prometheus Control

Landing Method 1: Send a Landing Command from the Prometheus Ground Station

1. Check the current UAV status.
2. Click Data Monitoring.
3. Click Land.

• The Prometheus-controlled landing begins.

Landing Method 2: Send a Landing Command with the H16 Remote Controller

• After the ground station is connected to the UAV and its status is normal, move SW4 to the bottom position to perform a Prometheus-controlled landing.

Landing Method 3: Land Manually

• Move SW2 to the top position to enter flight-controller-controlled POSITION mode. The UAV now responds to the remote controller.
• Slowly move the left stick all the way down to control the UAV's landing manually.
• After the UAV lands, wait for it to disarm automatically, or move SW3 to the bottom position to stop the propellers with one switch action.

Handling Abnormal UAV States

When the UAV Is Within the Inflated-Obstacle Area and No Target Point Has Been Published

• The UAV is surrounded by the inflated-obstacle area at takeoff.

• Manually move SW2 to the top position and fly the UAV out of the inflated-obstacle area.

1. Move SW2 to the middle position. SW3 and SW4 should be in the top position at this time.
2. The current Prometheus control status is RC_POS_CONTROL, which is the flight controller's OFFBOARD mode.
3. Click Video Monitoring.
4. Manually fly the UAV out of the inflated-obstacle area.
5. Click and hold the left mouse button on Hover at Current Point, then move SW2 to the bottom position. The UAV hovers at its current point and enters Prometheus-controlled COMMAND_CONTROL mode. (If you move SW2 to the bottom position without clicking Hover at Current Point, the UAV enters Prometheus-controlled COMMAND_CONTROL mode, flies above the initial point, and hovers.)

When the UAV Is Within the Inflated-Obstacle Area and a Target Point Has Already Been Published

• If the UAV is suddenly surrounded by the inflated-obstacle area, it hovers and cannot continue obstacle-avoidance planning.

• Manually move SW2 to the top position and fly the UAV out of the inflated-obstacle area.

• Slowly move SW2 to the bottom position to resume obstacle-avoidance planning.

Function Parameters

• The script used by this function is:

`/home/amov/p600_experiment/src/p600_experiment/scripts/lidar_ego_outdoor_s3.sh`

• The script contents are as follows:

gnome-terminal --window -e 'bash -c "roslaunch p600_experiment rplidar_s3.launch; exec bash"' \
--tab -e 'bash -c "sleep 2; roslaunch p600_experiment filter_lidar.launch; exec bash"' \
--tab -e 'bash -c "sleep 4; roslaunch p600_experiment scan_to_octomap_outdoor.launch; exec bash"' \
--tab -e 'bash -c "sleep 6; roslaunch p600_experiment ego_planner_octomap_outdoor.launch; exec bash"' \
--tab -e 'bash -c "sleep 8; source ~/.bashrc; roslaunch p600_experiment rviz_2dlidar.launch; exec bash"' \

LiDAR Driver Parameters

• The contents of rplidar_s3.launch are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/launch_planning/rplidar_s3.launch

<launch>
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <remap from="/scan" to="/uav1/scan"/>
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
  <param name="serial_baudrate"     type="int"    value="1000000"/>
  <param name="frame_id"            type="string" value="uav1/lidar_link"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  <param name="scan_frequency"      type="double" value="10.0"/>
  </node>
</launch>

LiDAR Filtering

• The contents of filter_lidar.launch are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/launch_planning/filter_lidar.launch

<launch>
  <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">
    <rosparam command="load" file="$(find p600_experiment)/config/filter_lidar.yaml"/>
    <remap from="/scan" to="/uav1/scan"/>
    <remap from="/scan_filtered" to="/uav1/scan_filtered"/>
  </node>
</launch>

Convert LiDAR Data to Octree Data

• The contents of scan_to_octomap_outdoor.launch are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/launch_planning/scan_to_octomap_outdoor.launch

<launch>
	
  <node pkg="plan_env" type="laser_to_pointcloud" name="laser_to_pointcloud" output="screen">
  	<remap from="/scan_filtered" to="/uav1/scan_filtered"/>
  </node>

  

  
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server" output="screen">
      
      <param name="resolution" value="0.1" />
      
      <param name="frame_id" type="string" value="world" />
      
      
      <param name="sensor_model/max_range" value="10.0" />
      
      
      
      <remap from="cloud_in" to="/uav1/prometheus/scan_point_cloud" />
      
      
      
      <param name = "base_frame_id" type = "string" value = "world" />
      
      <param name = "filter_ground" type = "bool" value = "true" />
      <param name = "ground_filter/distance" type = "double" value = "0.5" />
      <param name = "ground_filter/angle" type = "double" value = "0.7853" />
      <param name = "ground_filter/plane_distance" type = "double" value = "0.5" />
      
      
      
      
      
      
  </node>
</launch>

EGO-Planner Function Parameters

• The contents of ego_planner_octomap_outdoor.launch are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/launch_planning/ego_planner_octomap_outdoor.launch

<launch>
  <arg name="uav_id" default="1"/>

  
  <include file="$(find p600_experiment)/launch_planning/advanced_param_octomap_outdoor.xml">
    
    <arg name="uav_id" value="1"/>
    
    <arg name="map_size_x_" value="50.0"/>
    <arg name="map_size_y_" value="50.0"/>
    <arg name="map_size_z_" value="5"/>
    <arg name="map_origin_x_" value="-25.0"/>
		<arg name="map_origin_y_" value="-25.0"/>
    
    <arg name="ground_height" value="0.5"/>
    
    
    <arg name="odometry_topic" value="/prometheus/odom"/>
    
    <arg name="camera_pose_topic" value="/vins_estimator/camera_pose"/>
    <arg name="depth_topic" value="/camera/depth/image_rect_raw"/>
    <arg name="cx" value="326.2303466796875"/>
    <arg name="cy" value="235.4907684326172"/>
    <arg name="fx" value="387.4337158203125"/>
    <arg name="fy" value="387.4337158203125"/>
    
    <arg name="cloud_topic" value="/octomap_point_cloud_centers"/>
    
    <arg name="scan_topic" value="no_scan"/>
    
    <arg name="max_vel" value="0.6" />
    <arg name="max_acc" value="0.3" />
    
    <arg name="planning_horizon" value="15" /> 
    
    <arg name="use_distinctive_trajs" value="true" />
    
    
    
    <arg name="flight_type" value="1" />
    
    
    <arg name="point_num" value="4" />
    <arg name="point0_x" value="-6.0" />
    <arg name="point0_y" value="-3.0" />
    <arg name="point0_z" value="1.5" />

    <arg name="point1_x" value="0.0" />
    <arg name="point1_y" value="3.0" />
    <arg name="point1_z" value="1.5" />

    <arg name="point2_x" value="3.0" />
    <arg name="point2_y" value="-3.0" />
    <arg name="point2_z" value="1.5" />

    <arg name="point3_x" value="6.0" />
    <arg name="point3_y" value="-7.0" />
    <arg name="point3_z" value="1.5" />
    
    <arg name="realworld_experiment" value="true" />
  </include>

  
  
  <arg name="yaw_init" default="0.0"/>
  <node pkg="ego_planner" name="uav$(arg uav_id)_traj_server_for_prometheus" type="traj_server_for_prometheus" output="screen">
    <param name="uav_id" value="$(arg uav_id)"/>
    <param name="control_flag" value="0"/>
    <param name="traj_server/time_forward" value="1.0"/>
    <param name="traj_server/last_yaw" value="$(arg yaw_init)"/>
  </node>

</launch>

• The contents of the advanced_param_octomap_outdoor.xml configuration file are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/launch_planning/advanced_param_octomap_outdoor.xml

<launch>
  <arg name="map_size_x_"/>
  <arg name="map_size_y_"/>
  <arg name="map_size_z_"/>
  <arg name="map_origin_x_"/>
  <arg name="map_origin_y_"/>
  <arg name="ground_height"/>

  <arg name="odometry_topic"/>
  <arg name="camera_pose_topic"/>
  <arg name="depth_topic"/>
  <arg name="cloud_topic"/>
  <arg name="scan_topic"/>

  <arg name="cx"/>
  <arg name="cy"/>
  <arg name="fx"/>
  <arg name="fy"/>

  <arg name="max_vel"/>
  <arg name="max_acc"/>
  <arg name="planning_horizon"/>
  
  <arg name="point_num"/>
  <arg name="point0_x"/>
  <arg name="point0_y"/>
  <arg name="point0_z"/>
  <arg name="point1_x"/>
  <arg name="point1_y"/>
  <arg name="point1_z"/>
  <arg name="point2_x"/>
  <arg name="point2_y"/>
  <arg name="point2_z"/>
  <arg name="point3_x"/>
  <arg name="point3_y"/>
  <arg name="point3_z"/>

  <arg name="flight_type"/>
  <arg name="realworld_experiment"/>
  <arg name="use_distinctive_trajs"/>

  <arg name="uav_id"/>

  
  <node pkg="ego_planner" name="uav$(arg uav_id)_ego_planner_node" type="ego_planner_node" output="screen">
    
    <remap from="~odom_world" to="/uav$(arg uav_id)/$(arg odometry_topic)"/>
    
    <remap from="~planning/bspline" to = "/uav$(arg uav_id)/planning/bspline"/>
    
    <remap from="~planning/data_display" to = "/uav$(arg uav_id)/planning/data_display"/>
    
    <remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/>
    
    <remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>

    
    <remap from="~grid_map/odom" to="/uav$(arg uav_id)/$(arg odometry_topic)"/>
    
    
    <remap from="~grid_map/cloud" to="/$(arg cloud_topic)"/>
    
    <remap from="~grid_map/scan" to="/uav$(arg uav_id)/$(arg scan_topic)"/>
    
    <remap from="~grid_map/pose"   to = "/uav$(arg uav_id)/$(arg camera_pose_topic)"/> 
    
    
    <remap from="~grid_map/depth" to = "/$(arg depth_topic)"/>
    
    
    
    <param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
    
    <param name="fsm/thresh_replan_time" value="1.0" type="double"/>
    
    
    <param name="fsm/thresh_no_replan_meter" value="0.2" type="double"/>
    
    <param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/> 
    
    <param name="fsm/emergency_time" value="1.0" type="double"/>
    
    <param name="fsm/realworld_experiment" value="$(arg realworld_experiment)"/>
    <param name="fsm/fail_safe" value="true"/>
    
    <param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
    <param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
    <param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
    <param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
    <param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
    <param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
    <param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
    <param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
    <param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
    <param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
    <param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
    <param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
    <param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>

    
    
    <param name="grid_map/resolution"      value="0.1" /> 
    <param name="grid_map/map_size_x"   value="$(arg map_size_x_)" /> 
    <param name="grid_map/map_size_y"   value="$(arg map_size_y_)" /> 
    <param name="grid_map/map_size_z"   value="$(arg map_size_z_)" /> 
    <param name="grid_map/map_origin_x"   value="$(arg map_origin_x_)" /> 
    <param name="grid_map/map_origin_y"   value="$(arg map_origin_y_)" /> 
    
    
    <param name="grid_map/local_update_range_x"  value="5.5" /> 
    <param name="grid_map/local_update_range_y"  value="5.5" /> 
    <param name="grid_map/local_update_range_z"  value="4.5" /> 
    
    <param name="grid_map/obstacles_inflation"     value="0.4" /> 
    <param name="grid_map/local_map_margin" value="10"/>
    
    <param name="grid_map/ground_height"        value="$(arg ground_height)"/>
    
    <param name="grid_map/cx" value="$(arg cx)"/>
    <param name="grid_map/cy" value="$(arg cy)"/>
    <param name="grid_map/fx" value="$(arg fx)"/>
    <param name="grid_map/fy" value="$(arg fy)"/>
    
    <param name="grid_map/use_depth_filter" value="true"/>
    <param name="grid_map/depth_filter_tolerance" value="0.15"/>
    <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
    <param name="grid_map/depth_filter_mindist"   value="0.2"/>
    <param name="grid_map/depth_filter_margin"    value="2"/>
    <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
    <param name="grid_map/skip_pixel" value="2"/>
    
    <param name="grid_map/p_hit"  value="0.65"/>
    <param name="grid_map/p_miss" value="0.35"/>
    <param name="grid_map/p_min"  value="0.12"/>
    <param name="grid_map/p_max"  value="0.90"/>
    <param name="grid_map/p_occ"  value="0.80"/>
    <param name="grid_map/min_ray_length" value="0.1"/>
    <param name="grid_map/max_ray_length" value="4.5"/>

    <param name="grid_map/virtual_ceil_height"   value="2.0"/>
    <param name="grid_map/visualization_truncate_height"   value="1.8"/>
    <param name="grid_map/show_occ_time"  value="false"/>
    
    <param name="grid_map/pose_type"     value="1"/>  
    
    <param name="grid_map/frame_id"      value="world"/>

    
    
    <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
    
    <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
    
    <param name="manager/max_jerk" value="4" type="double"/>
    <param name="manager/control_points_distance" value="0.4" type="double"/>
    <param name="manager/feasibility_tolerance" value="0.05" type="double"/>
    <param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>
    <param name="manager/use_distinctive_trajs" value="$(arg use_distinctive_trajs)" type="bool"/>
    <param name="manager/drone_id" value="$(arg uav_id)"/>

    
    
    <param name="optimization/lambda_smooth" value="1.0" type="double"/>
    
    <param name="optimization/lambda_collision" value="0.5" type="double"/>
    
    <param name="optimization/lambda_feasibility" value="0.1" type="double"/>
    
    <param name="optimization/lambda_fitness" value="1.0" type="double"/>
    <param name="optimization/dist0" value="0.5" type="double"/>
    <param name="optimization/swarm_clearance" value="0.5" type="double"/>
    <param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>
    
    <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
    <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
    <param name="bspline/limit_ratio" value="1.1" type="double"/>
  </node>

</launch>

rviz Parameters

• The contents of rviz_2dlidar.launch are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/launch_planning/rviz_2dlidar.launch

<launch>
    
    <arg name="rviz_enable" default="true"/>
    <arg name="rivz_config" default="$(find p600_experiment)/launch_planning/2dlidar_mapping.rviz"/>
	
	<group if="$(arg rviz_enable)">
        <node type="rviz" name="rviz" pkg="rviz" args="-d $(arg rivz_config)"/>
    </group>
</launch>