EGO Path Planning

• Familiarity with indoor and outdoor position-hold flight is required first.
• Basic flight operations remain the same. For the MID360 version, LiDAR positioning must be started before operation.
• The following demonstration uses Windows.

Check Whether the UAV Status Is Normal

Start MID360 Positioning

• Connect to the UAV in Connection Settings (the positioning source must be switched to MID360 positioning).

1. Under Function Scripts, click P600_MID360 Positioning.

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

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

Check the Data

Handling Abnormal Data

1. It is normal for the initial position data to be close to 0.
2. If the initial position data has an excessive error greater than 0.5, it is abnormal. Perform the EKF2 reset below (this is usually caused by an initial data-fusion error or accumulated error after multiple flights).

MID360 positioning is normal

Initial Data

1. Click Function Scripts in the ground station.
2. Click ekf2 stop.
3. Click ekf2 start.

Start the Function Demo

Start the LiDAR Obstacle-Avoidance Script:

1. Under Function Scripts, click P600_MID360 LiDAR Obstacle Avoidance.
2. Check that the script has started.

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.
3. The ground station version is displayed.

Open the Local Visualization Interface

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

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, 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.

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. Enter Prometheus-controlled COMMAND_CONTROL mode. (Without clicking Hover at Current Point, move SW2 to the bottom position. 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 MID360 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.
5. 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, then click Send to plan a new route.

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.

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.

Function Parameters

• The script used by this function is:

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

• The script contents are as follows:

gnome-terminal --window -e 'bash -c "sleep 1; roslaunch p600_experiment ego_planner_basic_mid360.launch; exec bash"' \
--tab -e 'bash -c "sleep 2; source ~/.bashrc; roslaunch p600_experiment rviz_mid360.launch; exec bash"' \

LiDAR Driver Parameters

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

/home/amov/p600_experiment/src/p600_experiment/launch_mid360/msg_MID360.launch

<launch>

	
	<arg name="lvx_file_path" default="livox_test.lvx"/>
	<arg name="bd_list" default="47MDM580020260"/>
	<arg name="xfer_format" default="1"/>
	<arg name="multi_topic" default="0"/>
	<arg name="data_src" default="0"/>
	<arg name="publish_freq" default="10.0"/>
	<arg name="output_type" default="0"/>
	<arg name="rviz_enable" default="false"/>
	<arg name="rosbag_enable" default="false"/>
	<arg name="cmdline_arg" default="$(arg bd_list)"/>
	<arg name="msg_frame_id" default="livox_frame"/>
	<arg name="lidar_bag" default="true"/>
	<arg name="imu_bag" default="true"/>
	 

	<param name="xfer_format" value="$(arg xfer_format)"/>
	<param name="multi_topic" value="$(arg multi_topic)"/>
	<param name="data_src" value="$(arg data_src)"/>
	<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
	<param name="output_data_type" value="$(arg output_type)"/>
	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
	<param name="user_config_path" type="string" value="$(find p600_experiment)/config/mid360_config/MID360_config.json"/>
	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
	<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
	<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>

	<node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
	      type="livox_ros_driver2_node" required="true"
	      output="screen" args="$(arg cmdline_arg)"/>

	<group if="$(arg rosbag_enable)">
    	<node pkg="rosbag" type="record" name="record" output="screen"
          		args="-a"/>
    </group>

</launch>

LiDAR IP Configuration Parameters

• The contents of MID360_config.json are as follows:
• File path:

/home/amov/p600_experiment/src/p600_experiment/config/mid360_config/MID360_config.json

{
  "lidar_summary_info" : {
    "lidar_type": 8
  },
  "MID360": {
    "lidar_net_info" : {
      "cmd_data_port": 56100,
      "push_msg_port": 56200,
      "point_data_port": 56300,
      "imu_data_port": 56400,
      "log_data_port": 56500
    },
    "host_net_info" : {
      "cmd_data_ip" : "192.168.144.60",
      "cmd_data_port": 56101,
      "push_msg_ip": "192.168.144.60",
      "push_msg_port": 56201,
      "point_data_ip": "192.168.144.60",
      "point_data_port": 56301,
      "imu_data_ip" : "192.168.144.60",
      "imu_data_port": 56401,
      "log_data_ip" : "",
      "log_data_port": 56501
    }
  },
  "lidar_configs" : [
    {
      "ip" : "192.168.144.50",
      "pcl_data_type" : 1,
      "pattern_mode" : 0,
      "extrinsic_parameter" : {
        "roll": 0.0,
        "pitch": 20.0,
        "yaw": 0.0,
        "x": 0,
        "y": 0,
        "z": 0
      }
    }
  ]
}

EGO-Planner Function Parameters

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

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

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

  
  <include file="$(find ego_planner)/launch_for_prometheus/advanced_param_p600.xml">
    
    <arg name="uav_id" value="$(arg uav_id)"/>
    
    <arg name="map_size_x_" value="100.0"/>
    <arg name="map_size_y_" value="100.0"/>
    <arg name="map_size_z_" value="4.0"/>
    <arg name="map_origin_x_" value="-50.0"/>
    <arg name="map_origin_y_" value="-50.0"/>
    
    <arg name="ground_height" value="0.5"/>
    
    
    <arg name="odometry_topic" value="/mavros/local_position/odom"/>
    
    <arg name="camera_pose_topic" value="/depth/no_set"/>
    <arg name="depth_topic" value="/depth/image_rect_raw"/>
    <arg name="cx" value="321.04638671875"/>
    <arg name="cy" value="243.44969177246094"/>
    <arg name="fx" value="387.229248046875"/>
    <arg name="fy" value="387.229248046875"/>
    
    <arg name="cloud_topic" value="/uav$(arg uav_id)/octomap_point_cloud_centers"/>
    
    <arg name="scan_topic" value="no_scan"/>
    
    <arg name="max_vel" value="0.6" />
    <arg name="max_acc" value="3" />
    
    <arg name="planning_horizon" value="10.5" /> 
    
    <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_mid360.launch are as follows:
• File path:

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

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