What's wrong with this: "Priority queue is empty!!!!!!"

[donglianghere]

Is there anyone help me? very thanks!

[donglianghere]

[donglianghere]

I use OUSTER OS1-64 only, and make a 3d local and global planner.

[XuRobotics]

Hello, This message means that the local planner failed to find a trajectory to reach the goal. It seems that your local map is completely occupied around the robot. Maybe you can consider reduce the map dilation (in mapper.yaml) or change the takeoff location? See gazebo part of our wiki page for more details on how to customize those parameters.

[donglianghere]

Thanks! But i have made param "robot_r: 0.0" in mapper.yaml?

[donglianghere]

this is my mapper.yaml file:

This should contain tracker parameters

take_off_tracker: thrust_rate: 5.0 # if the robot cannot take off after reaching max_thrust, try increasing this (with caution if you're running real-robot experiments) max_thrust: 4.0 # if the robot cannot take off after reaching max_thrust, try increasing this (with caution if you're running real-robot experiments) min_thurst: -9.8 epsilon: 0.01

trajectory_tracker: max_pos_error: 0.75 # maximum allowed trajectory tracking error in meters (recommended value: 0.5-1.0) align_yaw: false # default false yaw_speed_magnitude: 0.3 # in rad/s the yaw alignment rate (recommended value: 0.1-0.3) use_lambda: true # pose error checking

trajectory_planner: debug: false # debug mode, default false verbose: false # verbose mode, default false max_v: 3.0 # x-y-direction maximum velocity,recommended value: for 2-D 9, for 3-D 3 max_a: 4.0 # x-y-direction maximum acceleration, recommended value: for 2-D 7, for 3-D 2 max_j: 2.0 # x-y-direction maximum jerk, recommended value: for 2-D 3, for 3-D 2 max_u: 2.0 # x-y-direction control inputs increase / decrease step, must be smaller than max_j, recommended value: for 2-D 3, for 3-D 1, for each axis jerk increment choice is -max_u, 0, max_u use_3d_local: true # local planner operates in 3D (true) or 2D (false)

use_3d_global: true # global planner operates in 3D (true, in this case use_3d_local has to be set as true) or 2D (false) z_cost_factor: 2 # global map z cost factor, this also influences the vertical semi field of view of global planner into 45-degree / z_cost_factor (cost_along_z = z_cost_factor * cost_along_x_or_y)

max_v_z: 1.5 # z-direction maximum velocity, recommended value: 1.5 max_a_z: 3.0 # z-direction maximum acceleration, recommended value: 3.0 max_j_z: 3.0 # z-direction maximum jerk, recommended value: 3.0 max_u_z: 3.0 # z-direction control inputs increase / decrease step, must be smaller than max_j, recommended value: 3.0 tol_pos: 3.0 # tolerance of distance for goal region, 0 means no tolerance, -1 means no limitation, recommended value: 3.0 for 2-D and 5.0 for 3-D global_goal_tol_vel: 2.0 # tolerance of velocity for goal region, recommended value: 2.0 for 2-D and 3.0 for 3-D global_goal_tol_acc: -1 # tolerance of acceleration for goal region, recommended value: -1 dt: 1.0 # execution time for each primitive, recommended value: 1.0 ndt: 1.0 # default 1 max_num: -1 # maxmum number of expansion, -1 means no limitation heuristic_weight: 50.0 # the larger heuristic_weight, the more emphasis on min time and the less emphasis on min control effort, recommended value: 50 for 2-D, 300 for 3-D vertical_semi_fov: 0.393 # vertical semi field of view (in rad), used for 3D planning only, value should lie in (0, pi/2), if 2D planning set it as 0 垂直方向半视角大小，单位是弧度，2D规划时，该值应该设为 0，3D规划是，Ouster OS1-64 应该为pi/8 即 0.392699082，22.5°

stopping_policy: acc_xy_des: 6.0 # stopping policy x-y maximum acceleration jerk_xy_des: 3.0 # stopping policy x-y maximum jerk acc_z_des: 2.0 # stopping policy z maximum acceleration jerk_z_des: 1.0 # stopping policy z maximum jerk acc_yaw_des: 0.05 # stopping policy yaw maximum angular acceleration in rad/(s^2)

local_global_server:

local planning horizon will be from robot to the local voxel map boundary

waypoint_reach_xy_threshold: 20.0 # x-y distance threshold, within which intermidiate waypoint is regarded as reached final_goal_reach_xy_threshold: 10.0 # within which final waypoint is regarded as reached, this should be smaller than waypoint_reach_threshold close_to_final_dist: 5 # distance-to-goal threshold, within which the velocity and acceleration checking will be turned on, -1 means no velocity checking at all local_plan_timeout_duration: 1.0 # local planner timeout in seconds max_local_plan_trials: 1 # should be set as 1 since state machine is now able to try multiple times after planner failures max_horizon: 10 # defulat 5, should be larger than local_plan_timeout_durationmax_local_plan_trialsreplan_rate global_planner_verbose: false # verbose mode, default false odom_frame: "quadrotor/odom" # odometry reference frame map_frame: "quadrotor/map" # SLAM reference frame (only for using two-reference-frame system, otherwise this should be set to be the same as odom_frame)

state_machine: replan_rate: 2.0 # replan frequency, calling local planner every (1/replan_rate) seconds, and global planner every (2/replan_rate) seconds max_replan_trials: 50 # max allowed trials to re-enter replanner

[XuRobotics]

I see, can you try changing increasing the resolution of the map, or change the takeoff location to some places that are more open?

Also the local planner is sensitive to how you discretize the control space, especially for 3D planning, otherwise it will fail/timeout frequently. You can play around with those parameters too, in tracker_params_mp.yaml. start with a small number for max_v, max_a, … We suggested a set of parameters for 2D & 3D planner respectively, you can choose between them. In general, 2D planning mode is recommended to start with.

[donglianghere]

Thank you @XuRobotics. I will try your advice soon.

[donglianghere]

@XuRobotics : Maybe another problem with the file "kr_autonomous_flight/autonomy_real/real_experiment_launch/config/trackers.yaml" I cannt find tracks "std_trackers/LineTrackerMinJerkAction" and "std_trackers/LineTrackerDistanceAction" and others, but i find tracks "kr_trackers/LineTrackerMinJerk" and "kr_trackers/LineTrackerDistance" , and i use the laters instead.

[donglianghere]

@XuRobotics In file "autonomy_core/map_plan/mapper/src/local_global_mapper.cpp" L222 to L225, Does it matter that i replace the sentences auto tf_map_lidar = tf_listener.LookupTransform( mapframe, lidarframe, cloud_header.stamp); auto tf_odom_lidar = tf_listener.LookupTransform( odomframe, lidarframe, cloud_header.stamp); with auto tf_map_lidar = tf_listener.LookupTransform( mapframe, lidarframe, ros::Time(0)); auto tf_odom_lidar = tf_listener.LookupTransform( odomframe, lidarframe, ros::Time(0));

[XuRobotics]

The tf change you made should be fine, since odom reference frame will be the same with map reference frame for the default setup (unless you are running a separate SLAM system that publishes the tf between them for drift correction)

[donglianghere]

@XuRobotics Thanks for your patience. I want to utilize the system in a room with the size of 7m X 10m X 3m, are there any params that must be tuned because of the big differences in the size?

[XuRobotics]

No problem! Several things off the top of my head that you can try are:

1. reduce the map dimension in the mapper.yaml to fit the scale of the environment you're doing the experiment in. Increase the resolution of the map (both local and gloal), to probably 0.1m since you have a small-scale environment.
2. you should add dilation back, since 0 dilation means the planner does not account for the robot size at all, and will be unsafe. You can try to turn on the dilation for the global map too. Your robot_r and robot_h in the mapper.yaml should be slightly larger than your actual robot size. All of those can be set in mapper.yaml file.
3. start with the 2D planning mode, reduce the planner speed, acceleration, jerk, and control to a small number (e.g. 1), and start from there. When that works, you may want to increase the higher-order first, e.g., max_v = 1, max_acc = 1, max_j = 2, max_u = 2.
4. when you feel confident with 2D planning mode, you can then switch to 3D mode. You can try to increase the vertical field of view of the planner (but should not be larger than your sensor's vertical field of view to guarantee safety). Also, you can reduce the replan rate if you're flying slowly. For example, if your local mapper ray-cast range is 10 meters and your max_v is 2, then there's no need to replan at 2 Hz at all.

For the tracker, all the trackers we use in the stack are included in either this repository or the kr_mav_control which is automatically built if you followed our instructions. So I don't think you need to change anything in the trackers.yaml file. It may include additional trackers that are not used in by the stack, and thus you cannot find it, but that should not cause any issues.

[donglianghere]

@XuRobotics Thank you very much, So detailed answer!!!