Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

the drone wobbles in and around the starting point #40

Open
yangy-98 opened this issue Feb 20, 2022 · 6 comments
Open

the drone wobbles in and around the starting point #40

yangy-98 opened this issue Feb 20, 2022 · 6 comments

Comments

@yangy-98
Copy link

Hello,
when I run the command:
roslaunch px4_fast_planner px4_fast_planner.launch

the terminal shows:

... logging to /home/njust/.ros/log/eb340e2c-9220-11ec-b2ca-f8633f2ed425/roslaunch-nuc-17513.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
started roslaunch server http://nuc:44437/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /camera_pose_publisher/child_frame: camera_link
 * /camera_pose_publisher/parent_frame: world
 * /camera_pose_publisher/pose_topic: camera/pose
 * /fast_planner_node/bspline/limit_acc: 2.5
 * /fast_planner_node/bspline/limit_ratio: 1.1
 * /fast_planner_node/bspline/limit_vel: 1.0
 * /fast_planner_node/fsm/act_map: False
 * /fast_planner_node/fsm/flight_type: 1
 * /fast_planner_node/fsm/thresh_no_replan: 2.0
 * /fast_planner_node/fsm/thresh_replan: 0.5
 * /fast_planner_node/fsm/waypoint0_x: 19.0
 * /fast_planner_node/fsm/waypoint0_y: 0.0
 * /fast_planner_node/fsm/waypoint0_z: 1.0
 * /fast_planner_node/fsm/waypoint1_x: 0.0
 * /fast_planner_node/fsm/waypoint1_y: 10.0
 * /fast_planner_node/fsm/waypoint1_z: 1.0
 * /fast_planner_node/fsm/waypoint2_x: -19.0
 * /fast_planner_node/fsm/waypoint2_y: 0.0
 * /fast_planner_node/fsm/waypoint2_z: 1.0
 * /fast_planner_node/fsm/waypoint_num: 2
 * /fast_planner_node/heading_planner/half_vert_num: 3
 * /fast_planner_node/heading_planner/lambda1: 2.0
 * /fast_planner_node/heading_planner/lambda2: 1.0
 * /fast_planner_node/heading_planner/max_yaw_rate: 1.04719753333
 * /fast_planner_node/heading_planner/w: 10.0
 * /fast_planner_node/heading_planner/yaw_diff: 0.0872664611111
 * /fast_planner_node/manager/clearance_threshold: 0.2
 * /fast_planner_node/manager/control_points_distance: 0.3
 * /fast_planner_node/manager/dynamic_environment: 0
 * /fast_planner_node/manager/local_segment_length: 7.0
 * /fast_planner_node/manager/max_acc: 2.5
 * /fast_planner_node/manager/max_jerk: 4.0
 * /fast_planner_node/manager/max_vel: 1.0
 * /fast_planner_node/manager/use_geometric_path: False
 * /fast_planner_node/manager/use_kinodynamic_path: False
 * /fast_planner_node/manager/use_optimization: True
 * /fast_planner_node/manager/use_topo_path: True
 * /fast_planner_node/optimization/algorithm1: 15
 * /fast_planner_node/optimization/algorithm2: 11
 * /fast_planner_node/optimization/dist0: 0.4
 * /fast_planner_node/optimization/dist1: 0.0
 * /fast_planner_node/optimization/lambda1: 10.0
 * /fast_planner_node/optimization/lambda2: 5.0
 * /fast_planner_node/optimization/lambda3: 0.0
 * /fast_planner_node/optimization/lambda4: 0.001
 * /fast_planner_node/optimization/lambda5: 1.5
 * /fast_planner_node/optimization/lambda6: 10.0
 * /fast_planner_node/optimization/lambda7: 20.0
 * /fast_planner_node/optimization/max_acc: 2.5
 * /fast_planner_node/optimization/max_iteration_num1: 2
 * /fast_planner_node/optimization/max_iteration_num2: 300
 * /fast_planner_node/optimization/max_iteration_time1: 0.0001
 * /fast_planner_node/optimization/max_iteration_time2: 0.005
 * /fast_planner_node/optimization/max_vel: 1.0
 * /fast_planner_node/optimization/order: 3
 * /fast_planner_node/planner_node/planner: 2
 * /fast_planner_node/sdf_map/cx: 320.5
 * /fast_planner_node/sdf_map/cy: 240.5
 * /fast_planner_node/sdf_map/depth_filter_margin: 2
 * /fast_planner_node/sdf_map/depth_filter_maxdist: 7
 * /fast_planner_node/sdf_map/depth_filter_mindist: 0.2
 * /fast_planner_node/sdf_map/depth_filter_tolerance: 0.15
 * /fast_planner_node/sdf_map/esdf_slice_height: 0.3
 * /fast_planner_node/sdf_map/frame_id: world
 * /fast_planner_node/sdf_map/fx: 343.496367536
 * /fast_planner_node/sdf_map/fy: 343.496367536
 * /fast_planner_node/sdf_map/ground_height: -1.0
 * /fast_planner_node/sdf_map/k_depth_scaling_factor: 1000.0
 * /fast_planner_node/sdf_map/local_bound_inflate: 0.5
 * /fast_planner_node/sdf_map/local_map_margin: 50
 * /fast_planner_node/sdf_map/local_update_range_x: 5.5
 * /fast_planner_node/sdf_map/local_update_range_y: 5.5
 * /fast_planner_node/sdf_map/local_update_range_z: 4.5
 * /fast_planner_node/sdf_map/map_size_x: 40.0
 * /fast_planner_node/sdf_map/map_size_y: 40.0
 * /fast_planner_node/sdf_map/map_size_z: 5.0
 * /fast_planner_node/sdf_map/max_ray_length: 4.5
 * /fast_planner_node/sdf_map/min_ray_length: 0.5
 * /fast_planner_node/sdf_map/obstacles_inflation: 0.099
 * /fast_planner_node/sdf_map/p_hit: 0.65
 * /fast_planner_node/sdf_map/p_max: 0.9
 * /fast_planner_node/sdf_map/p_min: 0.12
 * /fast_planner_node/sdf_map/p_miss: 0.35
 * /fast_planner_node/sdf_map/p_occ: 0.8
 * /fast_planner_node/sdf_map/pose_type: 1
 * /fast_planner_node/sdf_map/resolution: 0.1
 * /fast_planner_node/sdf_map/show_esdf_time: False
 * /fast_planner_node/sdf_map/show_occ_time: False
 * /fast_planner_node/sdf_map/skip_pixel: 2
 * /fast_planner_node/sdf_map/use_depth_filter: True
 * /fast_planner_node/sdf_map/virtual_ceil_height: 3.0
 * /fast_planner_node/sdf_map/visualization_truncate_height: 2.5
 * /fast_planner_node/topo_prm/clearance: 0.3
 * /fast_planner_node/topo_prm/max_raw_path2: 25
 * /fast_planner_node/topo_prm/max_raw_path: 300
 * /fast_planner_node/topo_prm/max_sample_num: 2000
 * /fast_planner_node/topo_prm/max_sample_time: 0.005
 * /fast_planner_node/topo_prm/parallel_shortcut: True
 * /fast_planner_node/topo_prm/ratio_to_short: 5.5
 * /fast_planner_node/topo_prm/reserve_num: 6
 * /fast_planner_node/topo_prm/sample_inflate_x: 1.0
 * /fast_planner_node/topo_prm/sample_inflate_y: 5.5
 * /fast_planner_node/topo_prm/sample_inflate_z: 1.0
 * /fast_planner_node/topo_prm/short_cut_num: 1
 * /gazebo/enable_ros_network: True
 * /geometric_controller/Kp_x: 6.0
 * /geometric_controller/Kp_y: 6.0
 * /geometric_controller/Kp_z: 10.0
 * /geometric_controller/Kv_x: 1.5
 * /geometric_controller/Kv_y: 1.5
 * /geometric_controller/Kv_z: 3.5
 * /geometric_controller/ctrl_mode: 2
 * /geometric_controller/enable_gazebo_state: True
 * /geometric_controller/enable_sim: True
 * /geometric_controller/init_pos_x: 0.0
 * /geometric_controller/init_pos_y: 0.0
 * /geometric_controller/init_pos_z: 1.0
 * /geometric_controller/mav_name: iris
 * /geometric_controller/max_acc: 2.5
 * /geometric_controller/velocity_yaw: False
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 408.0
 * /mavros/fake_gps/geo_origin/lat: 47.3667
 * /mavros/fake_gps/geo_origin/lon: 8.55
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /mavros/fake_gps/tf/child_frame_id: fix
 * /mavros/fake_gps/tf/frame_id: map
 * /mavros/fake_gps/tf/listen: False
 * /mavros/fake_gps/tf/rate_limit: 10.0
 * /mavros/fake_gps/tf/send: False
 * /mavros/fake_gps/use_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: udp://:14540@loca...
 * /mavros/gcs_url: 
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/gps_uere: 1.0
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: base_link
 * /mavros/global_position/tf/frame_id: map
 * /mavros/global_position/tf/global_frame_id: earth
 * /mavros/global_position/tf/send: False
 * /mavros/global_position/use_relative_alt: True
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /mavros/imu/frame_id: base_link
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/landing_target/camera/fov_x: 2.0071286398
 * /mavros/landing_target/camera/fov_y: 2.0071286398
 * /mavros/landing_target/image/height: 480
 * /mavros/landing_target/image/width: 640
 * /mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /mavros/landing_target/listen_lt: False
 * /mavros/landing_target/mav_frame: LOCAL_NED
 * /mavros/landing_target/target_size/x: 0.3
 * /mavros/landing_target/target_size/y: 0.3
 * /mavros/landing_target/tf/child_frame_id: camera_center
 * /mavros/landing_target/tf/frame_id: landing_target
 * /mavros/landing_target/tf/listen: False
 * /mavros/landing_target/tf/rate_limit: 10.0
 * /mavros/landing_target/tf/send: True
 * /mavros/local_position/frame_id: map
 * /mavros/local_position/tf/child_frame_id: base_link
 * /mavros/local_position/tf/frame_id: map
 * /mavros/local_position/tf/send: True
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mission/use_mission_item_int: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_thrust: False
 * /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /mavros/setpoint_attitude/tf/frame_id: map
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /mavros/setpoint_attitude/use_quaternion: False
 * /mavros/setpoint_position/mav_frame: LOCAL_NED
 * /mavros/setpoint_position/tf/child_frame_id: target_position
 * /mavros/setpoint_position/tf/frame_id: map
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/setpoint_raw/thrust_scaling: 1.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /mavros/startup_px4_usb_quirk: True
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/time/timesync_mode: MAVLINK
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: odom
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: True
 * /mavros/vision_speed/twist_cov: True
 * /mavros/wheel_odometry/child_frame_id: base_link
 * /mavros/wheel_odometry/count: 2
 * /mavros/wheel_odometry/frame_id: odom
 * /mavros/wheel_odometry/send_raw: True
 * /mavros/wheel_odometry/send_twist: False
 * /mavros/wheel_odometry/tf/child_frame_id: base_link
 * /mavros/wheel_odometry/tf/frame_id: odom
 * /mavros/wheel_odometry/tf/send: False
 * /mavros/wheel_odometry/use_rpm: False
 * /mavros/wheel_odometry/vel_error: 0.1
 * /mavros/wheel_odometry/wheel0/radius: 0.05
 * /mavros/wheel_odometry/wheel0/x: 0.0
 * /mavros/wheel_odometry/wheel0/y: -0.15
 * /mavros/wheel_odometry/wheel1/radius: 0.05
 * /mavros/wheel_odometry/wheel1/x: 0.0
 * /mavros/wheel_odometry/wheel1/y: 0.15
 * /rosdistro: melodic
 * /rosversion: 1.14.12
 * /traj_msg_converter/fast_planner_traj_topic: planning/ref_traj
 * /traj_msg_converter/traj_pub_topic: command/trajectory
 * /traj_server/traj_server/time_forward: 1.5
 * /use_sim_time: True
 * /waypoint_generator/waypoint_type: manual-lonely-way...

NODES
  /
    camera_pose_publisher (px4_fast_planner/camera_pose_publisher.py)
    fast_planner_node (plan_manage/fast_planner_node)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    geometric_controller (geometric_controller/geometric_controller_node)
    mavros (mavros/mavros_node)
    rqt_reconfigure (rqt_reconfigure/rqt_reconfigure)
    rvizvisualisation (rviz/rviz)
    sitl (px4/px4)
    tf_camera (tf/static_transform_publisher)
    traj_msg_converter (px4_fast_planner/trajectory_msg_converter.py)
    traj_server (plan_manage/traj_server)
    vehicle_spawn_nuc_17513_8915702001176790362 (gazebo_ros/spawn_model)
    waypoint_generator (waypoint_generator/waypoint_generator)
    world_map_linker (tf/static_transform_publisher)

auto-starting new master
process[master]: started with pid [17523]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to eb340e2c-9220-11ec-b2ca-f8633f2ed425
process[rosout-1]: started with pid [17534]
started core service [/rosout]
process[world_map_linker-2]: started with pid [17542]
process[tf_camera-3]: started with pid [17543]
process[sitl-4]: started with pid [17554]
INFO  [px4] Creating symlink /home/njust/Firmware/ROMFS/px4fmu_common -> /home/njust/.ros/etc
0 WARNING: setRealtimeSched failed (not run as root?)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
process[gazebo-5]: started with pid [17572]
process[gazebo_gui-6]: started with pid [17581]
process[vehicle_spawn_nuc_17513_8915702001176790362-7]: started with pid [17590]
process[mavros-8]: started with pid [17593]
INFO  [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
INFO  [simulator] Waiting for simulator to connect on TCP port 4560
process[fast_planner_node-9]: started with pid [17606]
process[traj_server-10]: started with pid [17614]
process[waypoint_generator-11]: started with pid [17617]
process[geometric_controller-12]: started with pid [17622]
process[rqt_reconfigure-13]: started with pid [17627]
process[camera_pose_publisher-14]: started with pid [17629]
process[traj_msg_converter-15]: started with pid [17635]
process[rvizvisualisation-16]: started with pid [17637]
[ INFO] [1645343063.946551459]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1645343063.955107417]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1645343063.955237708]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1645343063.955322683]: GCS bridge disabled
[ INFO] [1645343063.978374732]: Plugin 3dr_radio loaded
[ INFO] [1645343063.983463471]: Plugin 3dr_radio initialized
[ INFO] [1645343063.984487249]: Plugin actuator_control loaded
[ INFO] [1645343063.990217028]: Plugin actuator_control initialized
[ INFO] [1645343063.994984539]: Plugin adsb loaded
[ INFO] [1645343064.042044571]: Plugin adsb initialized
[ INFO] [1645343064.042206435]: Plugin altitude loaded
[ INFO] [1645343064.050109989]: Plugin altitude initialized
[ INFO] [1645343064.050250040]: Plugin cam_imu_sync loaded
[ INFO] [1645343064.051451386]: Plugin cam_imu_sync initialized
[ INFO] [1645343064.051579030]: Plugin camera loaded
[ INFO] [1645343064.058256564]: Plugin camera initialized
[ INFO] [1645343064.058408618]: Plugin command loaded
[ INFO] [1645343064.084802218]: Plugin command initialized
[ INFO] [1645343064.084947568]: Plugin companion_process_status loaded
[ INFO] [1645343064.103412163]: Plugin companion_process_status initialized
[ INFO] [1645343064.103553717]: Plugin debug_value loaded
[ INFO] [1645343064.136040285]: Plugin debug_value initialized
[ INFO] [1645343064.136110433]: Plugin distance_sensor blacklisted
[ INFO] [1645343064.136230449]: Plugin esc_status loaded
[ INFO] [1645343064.146330145]: Plugin esc_status initialized
[ INFO] [1645343064.146472963]: Plugin esc_telemetry loaded
[ INFO] [1645343064.147130778]: Plugin esc_telemetry initialized
[ INFO] [1645343064.147274474]: Plugin fake_gps loaded
[ INFO] [1645343064.231376028]: Plugin fake_gps initialized
[ INFO] [1645343064.231514153]: Plugin ftp loaded
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
[ INFO] [1645343064.275973382]: Plugin ftp initialized
[ INFO] [1645343064.276111703]: Plugin geofence loaded
[ INFO] [1645343064.287538938]: Plugin geofence initialized
[ INFO] [1645343064.287688403]: Plugin global_position loaded
[ INFO] [1645343064.361019951]: Plugin global_position initialized
[ INFO] [1645343064.361146509]: Plugin gps_input loaded
[ INFO] [1645343064.364639737]: Plugin gps_input initialized
[ INFO] [1645343064.364820175]: Plugin gps_rtk loaded
[ INFO] [1645343064.367707213]: Plugin gps_rtk initialized
[ INFO] [1645343064.367899555]: Plugin gps_status loaded
[ INFO] [1645343064.377333917]: Plugin gps_status initialized
[ INFO] [1645343064.377554218]: Plugin hil loaded
[ INFO] [1645343064.424574080]: Plugin hil initialized
[ INFO] [1645343064.424801264]: Plugin home_position loaded
[ INFO] [1645343064.431830864]: Plugin home_position initialized
[ INFO] [1645343064.432018631]: Plugin imu loaded
[ INFO] [1645343064.462481289]: Plugin imu initialized
[ INFO] [1645343064.462713493]: Plugin landing_target loaded
[ INFO] [1645343064.501729157]: Plugin landing_target initialized
[ INFO] [1645343064.501953049]: Plugin local_position loaded
[ INFO] [1645343064.517622431]: Plugin local_position initialized
[ INFO] [1645343064.517854420]: Plugin log_transfer loaded
[ INFO] [1645343064.523282175]: Plugin log_transfer initialized
[ INFO] [1645343064.523489691]: Plugin mag_calibration_status loaded
[ INFO] [1645343064.525732422]: Plugin mag_calibration_status initialized
[ INFO] [1645343064.525948884]: Plugin manual_control loaded
[ INFO] [1645343064.531319328]: Plugin manual_control initialized
[ INFO] [1645343064.531539073]: Plugin mocap_pose_estimate loaded
[ INFO] [1645343064.540432768]: Plugin mocap_pose_estimate initialized
[ INFO] [1645343064.540674830]: Plugin mount_control loaded
[ INFO] [1645343064.550353969]: Plugin mount_control initialized
[ INFO] [1645343064.550575753]: Plugin nav_controller_output loaded
[ INFO] [1645343064.551361622]: Plugin nav_controller_output initialized
[ INFO] [1645343064.551554976]: Plugin obstacle_distance loaded
[ INFO] [1645343064.571132641]: Plugin obstacle_distance initialized
[ INFO] [1645343064.571359873]: Plugin odom loaded
[ INFO] [1645343064.586577449]: Plugin odom initialized
[ INFO] [1645343064.586796156]: Plugin onboard_computer_status loaded
[ INFO] [1645343064.590728491]: Plugin onboard_computer_status initialized
[ INFO] [1645343064.590980306]: Plugin param loaded
[ INFO] [1645343064.599312502]: Plugin param initialized
[ INFO] [1645343064.599529226]: Plugin play_tune loaded
[ INFO] [1645343064.604612170]: Plugin play_tune initialized
[ INFO] [1645343064.604794776]: Plugin px4flow loaded
[ INFO] [1645343064.636937334]: Plugin px4flow initialized
[ INFO] [1645343064.637215023]: Plugin rallypoint loaded
[ INFO] [1645343064.647518891]: Plugin rallypoint initialized
[ INFO] [1645343064.647653341]: Plugin rangefinder blacklisted
[ INFO] [1645343064.647835286]: Plugin rc_io loaded
[ INFO] [1645343064.663854075]: Plugin rc_io initialized
[ INFO] [1645343064.663990157]: Plugin safety_area blacklisted
[ INFO] [1645343064.664160325]: Plugin setpoint_accel loaded
[ INFO] [1645343064.677111085]: Plugin setpoint_accel initialized
[ INFO] [1645343064.677364117]: Plugin setpoint_attitude loaded
[ INFO] [1645343064.715683271]: Plugin setpoint_attitude initialized
[ INFO] [1645343064.715868378]: Plugin setpoint_position loaded
INFO  [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0
[ INFO] [1645343064.775354149]: Plugin setpoint_position initialized
[ INFO] [1645343064.775593606]: Plugin setpoint_raw loaded
[ INFO] [1645343064.798244861]: Plugin setpoint_raw initialized
[ INFO] [1645343064.798478187]: Plugin setpoint_trajectory loaded
[ INFO] [1645343064.811059500]: Plugin setpoint_trajectory initialized
[ INFO] [1645343064.811288622]: Plugin setpoint_velocity loaded
[ INFO] [1645343064.833605783]: Plugin setpoint_velocity initialized
[ INFO] [1645343064.833861921]: Plugin sys_status loaded
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 remote port 14550
[ INFO] [1645343064.871425150]: Plugin sys_status initialized
[ INFO] [1645343064.871643251]: Plugin sys_time loaded
[ INFO] [1645343064.902398239]: TM: Timesync mode: MAVLINK
[ INFO] [1645343064.903783475]: TM: Not publishing sim time
[ INFO] [1645343064.908627434]: Plugin sys_time initialized
[ INFO] [1645343064.908845589]: Plugin terrain loaded
[ INFO] [1645343064.910758490]: Plugin terrain initialized
[ INFO] [1645343064.910974435]: Plugin trajectory loaded
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
[ INFO] [1645343064.938611232]: Plugin trajectory initialized
[ INFO] [1645343064.939797177]: Plugin tunnel loaded
[ INFO] [1645343064.950666329]: Plugin tunnel initialized
[ INFO] [1645343064.950836057]: Plugin vfr_hud loaded
[ INFO] [1645343064.954456116]: Plugin vfr_hud initialized
[ INFO] [1645343064.954540368]: Plugin vibration blacklisted
[ INFO] [1645343064.954665211]: Plugin vision_pose_estimate loaded
[ INFO] [1645343065.003085655]: Plugin vision_pose_estimate initialized
[ INFO] [1645343065.003334935]: Plugin vision_speed_estimate loaded
[ INFO] [1645343065.029039283]: Plugin vision_speed_estimate initialized
[ INFO] [1645343065.029300260]: Plugin waypoint loaded
[ INFO] [1645343065.054604623]: Plugin waypoint initialized
[ INFO] [1645343065.054767062]: Plugin wheel_odometry blacklisted
[ INFO] [1645343065.054943486]: Plugin wind_estimation loaded
[ INFO] [1645343065.058992475]: Plugin wind_estimation initialized
[ INFO] [1645343065.059192993]: Autostarting mavlink via USB on PX4
[ INFO] [1645343065.059291293]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1645343065.059370978]: Built-in MAVLink package version: 2022.1.5
[ INFO] [1645343065.059469007]: Known MAVLink dialects: common ardupilotmega ASLUAV all development icarous matrixpilot paparazzi standard uAvionix ualberta
[ INFO] [1645343065.059556398]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1645343065.061561492]: Finished loading Gazebo ROS API Plugin.
INFO  [logger] logger started (mode=all)
[ INFO] [1645343065.067812204]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
INFO  [logger] Start file log (type: full)
INFO  [logger] Opened full log file: ./log/2022-02-20/07_44_25.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
[ INFO] [1645343065.081118935]: udp0: Remote address: 127.0.0.1:14580
[ INFO] [1645343065.082116509]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1645343065.087612519]: FCU: [logger] file: ./log/2022-02-20/07_44_25.ulg
INFO  [px4] Startup script returned successfully
pxh> INFO  [mavlink] partner IP: 127.0.0.1
[ INFO] [1645343065.394119695]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1645343065.397274942]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[INFO] [1645343065.922150, 0.000000]: Loading model XML from file /home/njust/catkin_ws/src/px4_fast_planner/models/iris_depth_camera/iris_depth_camera.sdf
[INFO] [1645343065.924323, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1645343066.097164311]: GF: Using MISSION_ITEM_INT
[ INFO] [1645343066.097215339]: RP: Using MISSION_ITEM_INT
[ INFO] [1645343066.097263950]: WP: Using MISSION_ITEM_INT
[ INFO] [1645343066.097306055]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1645343066.097348038]: VER: 1.1: Flight software:     010a0100 (e0f016c2b3000000)
[ INFO] [1645343066.097376976]: VER: 1.1: Middleware software: 010a0100 (e0f016c2b3000000)
[ INFO] [1645343066.097407808]: VER: 1.1: OS software:         050400ff (427238133be2b0ec)
[ INFO] [1645343066.097440210]: VER: 1.1: Board hardware:      00000001
[ INFO] [1645343066.097544095]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1645343066.097568801]: VER: 1.1: UID:                 4954414c44494e4f
[ INFO] [1645343066.624257870]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1645343066.721117893]: Physics dynamic reconfigure ready.
[INFO] [1645343066.830700, 0.000000]: Calling service /gazebo/spawn_sdf_model
Warning [parser.cc:950] XML Element[vertical_fov], child of element[camera] not defined in SDF. Ignoring[vertical_fov]. You may have an incorrect SDF file, or an sdformat version that doesn't support this element.
[INFO] [1645343067.594226, 0.004000]: Spawn status: SpawnModel: Successfully spawned entity
[vehicle_spawn_nuc_17513_8915702001176790362-7] process has finished cleanly
log file: /home/njust/.ros/log/eb340e2c-9220-11ec-b2ca-f8633f2ed425/vehicle_spawn_nuc_17513_8915702001176790362-7*.log
[ INFO] [1645343068.142457299, 0.004000000]: Camera Plugin: The 'robotNamespace' param did not exit
[ INFO] [1645343068.147006163, 0.004000000]: Camera Plugin (ns = )  <tf_prefix_>, set to ""
INFO  [simulator] Simulator connected on TCP port 4560.
[ INFO] [1645343068.722826320, 0.012000000]: Waiting for home pose...
[ INFO] [1645343068.736453918, 0.024000000]: IMU: High resolution IMU detected!
INFO  [ecl/EKF] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
INFO  [ecl/EKF] 736000: EKF GPS checks passed (WGS-84 origin set)
[ INFO] [1645343069.982568489, 0.940000000]: IMU: Attitude quaternion IMU detected!
[ WARN] [1645343070.053717738, 1.004000000]: [Traj server]: ready.
[FSM]: from INIT to WAIT_TARGET
[ INFO] [1645343070.064806627, 1.012000000]: Home pose initialized to: position: 
  x: 0.000184849
  y: 0.00011755
  z: 0.00167428
orientation: 
  x: 0.0103552
  y: -0.0116411
  z: 0.0191484
  w: -0.999695

[ INFO] [1645343070.633063041, 1.512000000]: Got pose! Drone Ready to be armed.
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
INFO  [ecl/EKF] 5204000: EKF commencing GPS fusion
[ INFO] [1645343078.569473274, 6.004000000]: Offboard enabled
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
[ INFO] [1645343089.590134432, 12.012000000]: FCU: ARMED by Arm/Disarm component command
[ INFO] [1645343089.591626496, 12.012000000]: Vehicle armed
INFO  [commander] Takeoff detected
state: WAIT_TARGET
[ INFO] [1645343089.748329352, 12.060000000]: FCU: Takeoff detected
state: WAIT_TARGET
state: WAIT_TARGET
WARN  [mavlink] IGN REQUEST LIST: Busy
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343094.905536831, 15.000000000]: WP: mission received
[ERROR] [1645343094.909636081, 15.004000000]: FCU: IGN REQUEST LIST: Busy
[ERROR] [1645343094.969012656, 15.056000000]: FCU: IGN REQUEST LIST: Busy
[ WARN] [1645343096.506352936, 16.000000000]: RP: timeout, retries left 2
[ WARN] [1645343096.506594236, 16.000000000]: GF: timeout, retries left 2
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343096.507159579, 16.000000000]: RP: mission received
[ERROR] [1645343096.522541863, 16.000000000]: FCU: IGN REQUEST LIST: Busy
state: WAIT_TARGET
[ WARN] [1645343098.068787250, 17.000000000]: GF: timeout, retries left 1
[ INFO] [1645343098.069876863, 17.000000000]: GF: mission received
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
[rvizvisualisation-16] killing on exit
[traj_msg_converter-15] killing on exit
[camera_pose_publisher-14] killing on exit
[rqt_reconfigure-13] killing on exit
[geometric_controller-12] killing on exit
[traj_server-10] killing on exit
[fast_planner_node-9] killing on exit
[mavros-8] killing on exit
[gazebo_gui-6] killing on exit
[waypoint_generator-11] killing on exit
[gazebo-5] killing on exit
Traceback (most recent call last):
  File "/home/njust/catkin_ws/src/px4_fast_planner/scripts/camera_pose_publisher.py", line 41, in <module>
    rate.sleep()
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 103, in sleep
    sleep(self._remaining(curr_time))
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
    raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
[sitl-4] killing on exit

Exiting...
pxh> Shutting down
des manager
[tf_camera-3] killing on exit
[world_map_linker-2] killing on exit
[gazebo_gui-6] escalating to SIGTERM
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

the errors:
Warning [parser.cc:950] XML Element[vertical_fov], child of element[camera] not defined in SDF. Ignoring[vertical_fov]. You may have an incorrect SDF file, or an sdformat version that doesn't support this element.

and

WARN  [mavlink] IGN REQUEST LIST: Busy
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343094.905536831, 15.000000000]: WP: mission received
[ERROR] [1645343094.909636081, 15.004000000]: FCU: IGN REQUEST LIST: Busy
[ERROR] [1645343094.969012656, 15.056000000]: FCU: IGN REQUEST LIST: Busy
[ WARN] [1645343096.506352936, 16.000000000]: RP: timeout, retries left 2
[ WARN] [1645343096.506594236, 16.000000000]: GF: timeout, retries left 2
WARN  [mavlink] IGN REQUEST LIST: Busy

I have no idea with these problems. How to solve it? I look forward to your reply. Thank you!
@yangy-98
Copy link
Author

and in gazebo, the drone wobbles in and around the starting point.

@yangy-98
Copy link
Author

@mzahana I really need help, please

@calmelo
Copy link

calmelo commented Mar 20, 2022

Hello, when I run the command: roslaunch px4_fast_planner px4_fast_planner.launch

the terminal shows:

... logging to /home/njust/.ros/log/eb340e2c-9220-11ec-b2ca-f8633f2ed425/roslaunch-nuc-17513.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
started roslaunch server http://nuc:44437/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /camera_pose_publisher/child_frame: camera_link
 * /camera_pose_publisher/parent_frame: world
 * /camera_pose_publisher/pose_topic: camera/pose
 * /fast_planner_node/bspline/limit_acc: 2.5
 * /fast_planner_node/bspline/limit_ratio: 1.1
 * /fast_planner_node/bspline/limit_vel: 1.0
 * /fast_planner_node/fsm/act_map: False
 * /fast_planner_node/fsm/flight_type: 1
 * /fast_planner_node/fsm/thresh_no_replan: 2.0
 * /fast_planner_node/fsm/thresh_replan: 0.5
 * /fast_planner_node/fsm/waypoint0_x: 19.0
 * /fast_planner_node/fsm/waypoint0_y: 0.0
 * /fast_planner_node/fsm/waypoint0_z: 1.0
 * /fast_planner_node/fsm/waypoint1_x: 0.0
 * /fast_planner_node/fsm/waypoint1_y: 10.0
 * /fast_planner_node/fsm/waypoint1_z: 1.0
 * /fast_planner_node/fsm/waypoint2_x: -19.0
 * /fast_planner_node/fsm/waypoint2_y: 0.0
 * /fast_planner_node/fsm/waypoint2_z: 1.0
 * /fast_planner_node/fsm/waypoint_num: 2
 * /fast_planner_node/heading_planner/half_vert_num: 3
 * /fast_planner_node/heading_planner/lambda1: 2.0
 * /fast_planner_node/heading_planner/lambda2: 1.0
 * /fast_planner_node/heading_planner/max_yaw_rate: 1.04719753333
 * /fast_planner_node/heading_planner/w: 10.0
 * /fast_planner_node/heading_planner/yaw_diff: 0.0872664611111
 * /fast_planner_node/manager/clearance_threshold: 0.2
 * /fast_planner_node/manager/control_points_distance: 0.3
 * /fast_planner_node/manager/dynamic_environment: 0
 * /fast_planner_node/manager/local_segment_length: 7.0
 * /fast_planner_node/manager/max_acc: 2.5
 * /fast_planner_node/manager/max_jerk: 4.0
 * /fast_planner_node/manager/max_vel: 1.0
 * /fast_planner_node/manager/use_geometric_path: False
 * /fast_planner_node/manager/use_kinodynamic_path: False
 * /fast_planner_node/manager/use_optimization: True
 * /fast_planner_node/manager/use_topo_path: True
 * /fast_planner_node/optimization/algorithm1: 15
 * /fast_planner_node/optimization/algorithm2: 11
 * /fast_planner_node/optimization/dist0: 0.4
 * /fast_planner_node/optimization/dist1: 0.0
 * /fast_planner_node/optimization/lambda1: 10.0
 * /fast_planner_node/optimization/lambda2: 5.0
 * /fast_planner_node/optimization/lambda3: 0.0
 * /fast_planner_node/optimization/lambda4: 0.001
 * /fast_planner_node/optimization/lambda5: 1.5
 * /fast_planner_node/optimization/lambda6: 10.0
 * /fast_planner_node/optimization/lambda7: 20.0
 * /fast_planner_node/optimization/max_acc: 2.5
 * /fast_planner_node/optimization/max_iteration_num1: 2
 * /fast_planner_node/optimization/max_iteration_num2: 300
 * /fast_planner_node/optimization/max_iteration_time1: 0.0001
 * /fast_planner_node/optimization/max_iteration_time2: 0.005
 * /fast_planner_node/optimization/max_vel: 1.0
 * /fast_planner_node/optimization/order: 3
 * /fast_planner_node/planner_node/planner: 2
 * /fast_planner_node/sdf_map/cx: 320.5
 * /fast_planner_node/sdf_map/cy: 240.5
 * /fast_planner_node/sdf_map/depth_filter_margin: 2
 * /fast_planner_node/sdf_map/depth_filter_maxdist: 7
 * /fast_planner_node/sdf_map/depth_filter_mindist: 0.2
 * /fast_planner_node/sdf_map/depth_filter_tolerance: 0.15
 * /fast_planner_node/sdf_map/esdf_slice_height: 0.3
 * /fast_planner_node/sdf_map/frame_id: world
 * /fast_planner_node/sdf_map/fx: 343.496367536
 * /fast_planner_node/sdf_map/fy: 343.496367536
 * /fast_planner_node/sdf_map/ground_height: -1.0
 * /fast_planner_node/sdf_map/k_depth_scaling_factor: 1000.0
 * /fast_planner_node/sdf_map/local_bound_inflate: 0.5
 * /fast_planner_node/sdf_map/local_map_margin: 50
 * /fast_planner_node/sdf_map/local_update_range_x: 5.5
 * /fast_planner_node/sdf_map/local_update_range_y: 5.5
 * /fast_planner_node/sdf_map/local_update_range_z: 4.5
 * /fast_planner_node/sdf_map/map_size_x: 40.0
 * /fast_planner_node/sdf_map/map_size_y: 40.0
 * /fast_planner_node/sdf_map/map_size_z: 5.0
 * /fast_planner_node/sdf_map/max_ray_length: 4.5
 * /fast_planner_node/sdf_map/min_ray_length: 0.5
 * /fast_planner_node/sdf_map/obstacles_inflation: 0.099
 * /fast_planner_node/sdf_map/p_hit: 0.65
 * /fast_planner_node/sdf_map/p_max: 0.9
 * /fast_planner_node/sdf_map/p_min: 0.12
 * /fast_planner_node/sdf_map/p_miss: 0.35
 * /fast_planner_node/sdf_map/p_occ: 0.8
 * /fast_planner_node/sdf_map/pose_type: 1
 * /fast_planner_node/sdf_map/resolution: 0.1
 * /fast_planner_node/sdf_map/show_esdf_time: False
 * /fast_planner_node/sdf_map/show_occ_time: False
 * /fast_planner_node/sdf_map/skip_pixel: 2
 * /fast_planner_node/sdf_map/use_depth_filter: True
 * /fast_planner_node/sdf_map/virtual_ceil_height: 3.0
 * /fast_planner_node/sdf_map/visualization_truncate_height: 2.5
 * /fast_planner_node/topo_prm/clearance: 0.3
 * /fast_planner_node/topo_prm/max_raw_path2: 25
 * /fast_planner_node/topo_prm/max_raw_path: 300
 * /fast_planner_node/topo_prm/max_sample_num: 2000
 * /fast_planner_node/topo_prm/max_sample_time: 0.005
 * /fast_planner_node/topo_prm/parallel_shortcut: True
 * /fast_planner_node/topo_prm/ratio_to_short: 5.5
 * /fast_planner_node/topo_prm/reserve_num: 6
 * /fast_planner_node/topo_prm/sample_inflate_x: 1.0
 * /fast_planner_node/topo_prm/sample_inflate_y: 5.5
 * /fast_planner_node/topo_prm/sample_inflate_z: 1.0
 * /fast_planner_node/topo_prm/short_cut_num: 1
 * /gazebo/enable_ros_network: True
 * /geometric_controller/Kp_x: 6.0
 * /geometric_controller/Kp_y: 6.0
 * /geometric_controller/Kp_z: 10.0
 * /geometric_controller/Kv_x: 1.5
 * /geometric_controller/Kv_y: 1.5
 * /geometric_controller/Kv_z: 3.5
 * /geometric_controller/ctrl_mode: 2
 * /geometric_controller/enable_gazebo_state: True
 * /geometric_controller/enable_sim: True
 * /geometric_controller/init_pos_x: 0.0
 * /geometric_controller/init_pos_y: 0.0
 * /geometric_controller/init_pos_z: 1.0
 * /geometric_controller/mav_name: iris
 * /geometric_controller/max_acc: 2.5
 * /geometric_controller/velocity_yaw: False
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 408.0
 * /mavros/fake_gps/geo_origin/lat: 47.3667
 * /mavros/fake_gps/geo_origin/lon: 8.55
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /mavros/fake_gps/tf/child_frame_id: fix
 * /mavros/fake_gps/tf/frame_id: map
 * /mavros/fake_gps/tf/listen: False
 * /mavros/fake_gps/tf/rate_limit: 10.0
 * /mavros/fake_gps/tf/send: False
 * /mavros/fake_gps/use_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: udp://:14540@loca...
 * /mavros/gcs_url: 
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/gps_uere: 1.0
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: base_link
 * /mavros/global_position/tf/frame_id: map
 * /mavros/global_position/tf/global_frame_id: earth
 * /mavros/global_position/tf/send: False
 * /mavros/global_position/use_relative_alt: True
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /mavros/imu/frame_id: base_link
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/landing_target/camera/fov_x: 2.0071286398
 * /mavros/landing_target/camera/fov_y: 2.0071286398
 * /mavros/landing_target/image/height: 480
 * /mavros/landing_target/image/width: 640
 * /mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /mavros/landing_target/listen_lt: False
 * /mavros/landing_target/mav_frame: LOCAL_NED
 * /mavros/landing_target/target_size/x: 0.3
 * /mavros/landing_target/target_size/y: 0.3
 * /mavros/landing_target/tf/child_frame_id: camera_center
 * /mavros/landing_target/tf/frame_id: landing_target
 * /mavros/landing_target/tf/listen: False
 * /mavros/landing_target/tf/rate_limit: 10.0
 * /mavros/landing_target/tf/send: True
 * /mavros/local_position/frame_id: map
 * /mavros/local_position/tf/child_frame_id: base_link
 * /mavros/local_position/tf/frame_id: map
 * /mavros/local_position/tf/send: True
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mission/use_mission_item_int: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_thrust: False
 * /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /mavros/setpoint_attitude/tf/frame_id: map
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /mavros/setpoint_attitude/use_quaternion: False
 * /mavros/setpoint_position/mav_frame: LOCAL_NED
 * /mavros/setpoint_position/tf/child_frame_id: target_position
 * /mavros/setpoint_position/tf/frame_id: map
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/setpoint_raw/thrust_scaling: 1.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /mavros/startup_px4_usb_quirk: True
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/time/timesync_mode: MAVLINK
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: odom
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: True
 * /mavros/vision_speed/twist_cov: True
 * /mavros/wheel_odometry/child_frame_id: base_link
 * /mavros/wheel_odometry/count: 2
 * /mavros/wheel_odometry/frame_id: odom
 * /mavros/wheel_odometry/send_raw: True
 * /mavros/wheel_odometry/send_twist: False
 * /mavros/wheel_odometry/tf/child_frame_id: base_link
 * /mavros/wheel_odometry/tf/frame_id: odom
 * /mavros/wheel_odometry/tf/send: False
 * /mavros/wheel_odometry/use_rpm: False
 * /mavros/wheel_odometry/vel_error: 0.1
 * /mavros/wheel_odometry/wheel0/radius: 0.05
 * /mavros/wheel_odometry/wheel0/x: 0.0
 * /mavros/wheel_odometry/wheel0/y: -0.15
 * /mavros/wheel_odometry/wheel1/radius: 0.05
 * /mavros/wheel_odometry/wheel1/x: 0.0
 * /mavros/wheel_odometry/wheel1/y: 0.15
 * /rosdistro: melodic
 * /rosversion: 1.14.12
 * /traj_msg_converter/fast_planner_traj_topic: planning/ref_traj
 * /traj_msg_converter/traj_pub_topic: command/trajectory
 * /traj_server/traj_server/time_forward: 1.5
 * /use_sim_time: True
 * /waypoint_generator/waypoint_type: manual-lonely-way...

NODES
  /
    camera_pose_publisher (px4_fast_planner/camera_pose_publisher.py)
    fast_planner_node (plan_manage/fast_planner_node)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    geometric_controller (geometric_controller/geometric_controller_node)
    mavros (mavros/mavros_node)
    rqt_reconfigure (rqt_reconfigure/rqt_reconfigure)
    rvizvisualisation (rviz/rviz)
    sitl (px4/px4)
    tf_camera (tf/static_transform_publisher)
    traj_msg_converter (px4_fast_planner/trajectory_msg_converter.py)
    traj_server (plan_manage/traj_server)
    vehicle_spawn_nuc_17513_8915702001176790362 (gazebo_ros/spawn_model)
    waypoint_generator (waypoint_generator/waypoint_generator)
    world_map_linker (tf/static_transform_publisher)

auto-starting new master
process[master]: started with pid [17523]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to eb340e2c-9220-11ec-b2ca-f8633f2ed425
process[rosout-1]: started with pid [17534]
started core service [/rosout]
process[world_map_linker-2]: started with pid [17542]
process[tf_camera-3]: started with pid [17543]
process[sitl-4]: started with pid [17554]
INFO  [px4] Creating symlink /home/njust/Firmware/ROMFS/px4fmu_common -> /home/njust/.ros/etc
0 WARNING: setRealtimeSched failed (not run as root?)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
process[gazebo-5]: started with pid [17572]
process[gazebo_gui-6]: started with pid [17581]
process[vehicle_spawn_nuc_17513_8915702001176790362-7]: started with pid [17590]
process[mavros-8]: started with pid [17593]
INFO  [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
INFO  [simulator] Waiting for simulator to connect on TCP port 4560
process[fast_planner_node-9]: started with pid [17606]
process[traj_server-10]: started with pid [17614]
process[waypoint_generator-11]: started with pid [17617]
process[geometric_controller-12]: started with pid [17622]
process[rqt_reconfigure-13]: started with pid [17627]
process[camera_pose_publisher-14]: started with pid [17629]
process[traj_msg_converter-15]: started with pid [17635]
process[rvizvisualisation-16]: started with pid [17637]
[ INFO] [1645343063.946551459]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1645343063.955107417]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1645343063.955237708]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1645343063.955322683]: GCS bridge disabled
[ INFO] [1645343063.978374732]: Plugin 3dr_radio loaded
[ INFO] [1645343063.983463471]: Plugin 3dr_radio initialized
[ INFO] [1645343063.984487249]: Plugin actuator_control loaded
[ INFO] [1645343063.990217028]: Plugin actuator_control initialized
[ INFO] [1645343063.994984539]: Plugin adsb loaded
[ INFO] [1645343064.042044571]: Plugin adsb initialized
[ INFO] [1645343064.042206435]: Plugin altitude loaded
[ INFO] [1645343064.050109989]: Plugin altitude initialized
[ INFO] [1645343064.050250040]: Plugin cam_imu_sync loaded
[ INFO] [1645343064.051451386]: Plugin cam_imu_sync initialized
[ INFO] [1645343064.051579030]: Plugin camera loaded
[ INFO] [1645343064.058256564]: Plugin camera initialized
[ INFO] [1645343064.058408618]: Plugin command loaded
[ INFO] [1645343064.084802218]: Plugin command initialized
[ INFO] [1645343064.084947568]: Plugin companion_process_status loaded
[ INFO] [1645343064.103412163]: Plugin companion_process_status initialized
[ INFO] [1645343064.103553717]: Plugin debug_value loaded
[ INFO] [1645343064.136040285]: Plugin debug_value initialized
[ INFO] [1645343064.136110433]: Plugin distance_sensor blacklisted
[ INFO] [1645343064.136230449]: Plugin esc_status loaded
[ INFO] [1645343064.146330145]: Plugin esc_status initialized
[ INFO] [1645343064.146472963]: Plugin esc_telemetry loaded
[ INFO] [1645343064.147130778]: Plugin esc_telemetry initialized
[ INFO] [1645343064.147274474]: Plugin fake_gps loaded
[ INFO] [1645343064.231376028]: Plugin fake_gps initialized
[ INFO] [1645343064.231514153]: Plugin ftp loaded
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
[ INFO] [1645343064.275973382]: Plugin ftp initialized
[ INFO] [1645343064.276111703]: Plugin geofence loaded
[ INFO] [1645343064.287538938]: Plugin geofence initialized
[ INFO] [1645343064.287688403]: Plugin global_position loaded
[ INFO] [1645343064.361019951]: Plugin global_position initialized
[ INFO] [1645343064.361146509]: Plugin gps_input loaded
[ INFO] [1645343064.364639737]: Plugin gps_input initialized
[ INFO] [1645343064.364820175]: Plugin gps_rtk loaded
[ INFO] [1645343064.367707213]: Plugin gps_rtk initialized
[ INFO] [1645343064.367899555]: Plugin gps_status loaded
[ INFO] [1645343064.377333917]: Plugin gps_status initialized
[ INFO] [1645343064.377554218]: Plugin hil loaded
[ INFO] [1645343064.424574080]: Plugin hil initialized
[ INFO] [1645343064.424801264]: Plugin home_position loaded
[ INFO] [1645343064.431830864]: Plugin home_position initialized
[ INFO] [1645343064.432018631]: Plugin imu loaded
[ INFO] [1645343064.462481289]: Plugin imu initialized
[ INFO] [1645343064.462713493]: Plugin landing_target loaded
[ INFO] [1645343064.501729157]: Plugin landing_target initialized
[ INFO] [1645343064.501953049]: Plugin local_position loaded
[ INFO] [1645343064.517622431]: Plugin local_position initialized
[ INFO] [1645343064.517854420]: Plugin log_transfer loaded
[ INFO] [1645343064.523282175]: Plugin log_transfer initialized
[ INFO] [1645343064.523489691]: Plugin mag_calibration_status loaded
[ INFO] [1645343064.525732422]: Plugin mag_calibration_status initialized
[ INFO] [1645343064.525948884]: Plugin manual_control loaded
[ INFO] [1645343064.531319328]: Plugin manual_control initialized
[ INFO] [1645343064.531539073]: Plugin mocap_pose_estimate loaded
[ INFO] [1645343064.540432768]: Plugin mocap_pose_estimate initialized
[ INFO] [1645343064.540674830]: Plugin mount_control loaded
[ INFO] [1645343064.550353969]: Plugin mount_control initialized
[ INFO] [1645343064.550575753]: Plugin nav_controller_output loaded
[ INFO] [1645343064.551361622]: Plugin nav_controller_output initialized
[ INFO] [1645343064.551554976]: Plugin obstacle_distance loaded
[ INFO] [1645343064.571132641]: Plugin obstacle_distance initialized
[ INFO] [1645343064.571359873]: Plugin odom loaded
[ INFO] [1645343064.586577449]: Plugin odom initialized
[ INFO] [1645343064.586796156]: Plugin onboard_computer_status loaded
[ INFO] [1645343064.590728491]: Plugin onboard_computer_status initialized
[ INFO] [1645343064.590980306]: Plugin param loaded
[ INFO] [1645343064.599312502]: Plugin param initialized
[ INFO] [1645343064.599529226]: Plugin play_tune loaded
[ INFO] [1645343064.604612170]: Plugin play_tune initialized
[ INFO] [1645343064.604794776]: Plugin px4flow loaded
[ INFO] [1645343064.636937334]: Plugin px4flow initialized
[ INFO] [1645343064.637215023]: Plugin rallypoint loaded
[ INFO] [1645343064.647518891]: Plugin rallypoint initialized
[ INFO] [1645343064.647653341]: Plugin rangefinder blacklisted
[ INFO] [1645343064.647835286]: Plugin rc_io loaded
[ INFO] [1645343064.663854075]: Plugin rc_io initialized
[ INFO] [1645343064.663990157]: Plugin safety_area blacklisted
[ INFO] [1645343064.664160325]: Plugin setpoint_accel loaded
[ INFO] [1645343064.677111085]: Plugin setpoint_accel initialized
[ INFO] [1645343064.677364117]: Plugin setpoint_attitude loaded
[ INFO] [1645343064.715683271]: Plugin setpoint_attitude initialized
[ INFO] [1645343064.715868378]: Plugin setpoint_position loaded
INFO  [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0
[ INFO] [1645343064.775354149]: Plugin setpoint_position initialized
[ INFO] [1645343064.775593606]: Plugin setpoint_raw loaded
[ INFO] [1645343064.798244861]: Plugin setpoint_raw initialized
[ INFO] [1645343064.798478187]: Plugin setpoint_trajectory loaded
[ INFO] [1645343064.811059500]: Plugin setpoint_trajectory initialized
[ INFO] [1645343064.811288622]: Plugin setpoint_velocity loaded
[ INFO] [1645343064.833605783]: Plugin setpoint_velocity initialized
[ INFO] [1645343064.833861921]: Plugin sys_status loaded
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 remote port 14550
[ INFO] [1645343064.871425150]: Plugin sys_status initialized
[ INFO] [1645343064.871643251]: Plugin sys_time loaded
[ INFO] [1645343064.902398239]: TM: Timesync mode: MAVLINK
[ INFO] [1645343064.903783475]: TM: Not publishing sim time
[ INFO] [1645343064.908627434]: Plugin sys_time initialized
[ INFO] [1645343064.908845589]: Plugin terrain loaded
[ INFO] [1645343064.910758490]: Plugin terrain initialized
[ INFO] [1645343064.910974435]: Plugin trajectory loaded
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
[ INFO] [1645343064.938611232]: Plugin trajectory initialized
[ INFO] [1645343064.939797177]: Plugin tunnel loaded
[ INFO] [1645343064.950666329]: Plugin tunnel initialized
[ INFO] [1645343064.950836057]: Plugin vfr_hud loaded
[ INFO] [1645343064.954456116]: Plugin vfr_hud initialized
[ INFO] [1645343064.954540368]: Plugin vibration blacklisted
[ INFO] [1645343064.954665211]: Plugin vision_pose_estimate loaded
[ INFO] [1645343065.003085655]: Plugin vision_pose_estimate initialized
[ INFO] [1645343065.003334935]: Plugin vision_speed_estimate loaded
[ INFO] [1645343065.029039283]: Plugin vision_speed_estimate initialized
[ INFO] [1645343065.029300260]: Plugin waypoint loaded
[ INFO] [1645343065.054604623]: Plugin waypoint initialized
[ INFO] [1645343065.054767062]: Plugin wheel_odometry blacklisted
[ INFO] [1645343065.054943486]: Plugin wind_estimation loaded
[ INFO] [1645343065.058992475]: Plugin wind_estimation initialized
[ INFO] [1645343065.059192993]: Autostarting mavlink via USB on PX4
[ INFO] [1645343065.059291293]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1645343065.059370978]: Built-in MAVLink package version: 2022.1.5
[ INFO] [1645343065.059469007]: Known MAVLink dialects: common ardupilotmega ASLUAV all development icarous matrixpilot paparazzi standard uAvionix ualberta
[ INFO] [1645343065.059556398]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1645343065.061561492]: Finished loading Gazebo ROS API Plugin.
INFO  [logger] logger started (mode=all)
[ INFO] [1645343065.067812204]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
INFO  [logger] Start file log (type: full)
INFO  [logger] Opened full log file: ./log/2022-02-20/07_44_25.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
[ INFO] [1645343065.081118935]: udp0: Remote address: 127.0.0.1:14580
[ INFO] [1645343065.082116509]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1645343065.087612519]: FCU: [logger] file: ./log/2022-02-20/07_44_25.ulg
INFO  [px4] Startup script returned successfully
pxh> INFO  [mavlink] partner IP: 127.0.0.1
[ INFO] [1645343065.394119695]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1645343065.397274942]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[INFO] [1645343065.922150, 0.000000]: Loading model XML from file /home/njust/catkin_ws/src/px4_fast_planner/models/iris_depth_camera/iris_depth_camera.sdf
[INFO] [1645343065.924323, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1645343066.097164311]: GF: Using MISSION_ITEM_INT
[ INFO] [1645343066.097215339]: RP: Using MISSION_ITEM_INT
[ INFO] [1645343066.097263950]: WP: Using MISSION_ITEM_INT
[ INFO] [1645343066.097306055]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1645343066.097348038]: VER: 1.1: Flight software:     010a0100 (e0f016c2b3000000)
[ INFO] [1645343066.097376976]: VER: 1.1: Middleware software: 010a0100 (e0f016c2b3000000)
[ INFO] [1645343066.097407808]: VER: 1.1: OS software:         050400ff (427238133be2b0ec)
[ INFO] [1645343066.097440210]: VER: 1.1: Board hardware:      00000001
[ INFO] [1645343066.097544095]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1645343066.097568801]: VER: 1.1: UID:                 4954414c44494e4f
[ INFO] [1645343066.624257870]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1645343066.721117893]: Physics dynamic reconfigure ready.
[INFO] [1645343066.830700, 0.000000]: Calling service /gazebo/spawn_sdf_model
Warning [parser.cc:950] XML Element[vertical_fov], child of element[camera] not defined in SDF. Ignoring[vertical_fov]. You may have an incorrect SDF file, or an sdformat version that doesn't support this element.
[INFO] [1645343067.594226, 0.004000]: Spawn status: SpawnModel: Successfully spawned entity
[vehicle_spawn_nuc_17513_8915702001176790362-7] process has finished cleanly
log file: /home/njust/.ros/log/eb340e2c-9220-11ec-b2ca-f8633f2ed425/vehicle_spawn_nuc_17513_8915702001176790362-7*.log
[ INFO] [1645343068.142457299, 0.004000000]: Camera Plugin: The 'robotNamespace' param did not exit
[ INFO] [1645343068.147006163, 0.004000000]: Camera Plugin (ns = )  <tf_prefix_>, set to ""
INFO  [simulator] Simulator connected on TCP port 4560.
[ INFO] [1645343068.722826320, 0.012000000]: Waiting for home pose...
[ INFO] [1645343068.736453918, 0.024000000]: IMU: High resolution IMU detected!
INFO  [ecl/EKF] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
INFO  [ecl/EKF] 736000: EKF GPS checks passed (WGS-84 origin set)
[ INFO] [1645343069.982568489, 0.940000000]: IMU: Attitude quaternion IMU detected!
[ WARN] [1645343070.053717738, 1.004000000]: [Traj server]: ready.
[FSM]: from INIT to WAIT_TARGET
[ INFO] [1645343070.064806627, 1.012000000]: Home pose initialized to: position: 
  x: 0.000184849
  y: 0.00011755
  z: 0.00167428
orientation: 
  x: 0.0103552
  y: -0.0116411
  z: 0.0191484
  w: -0.999695

[ INFO] [1645343070.633063041, 1.512000000]: Got pose! Drone Ready to be armed.
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
INFO  [ecl/EKF] 5204000: EKF commencing GPS fusion
[ INFO] [1645343078.569473274, 6.004000000]: Offboard enabled
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
[ INFO] [1645343089.590134432, 12.012000000]: FCU: ARMED by Arm/Disarm component command
[ INFO] [1645343089.591626496, 12.012000000]: Vehicle armed
INFO  [commander] Takeoff detected
state: WAIT_TARGET
[ INFO] [1645343089.748329352, 12.060000000]: FCU: Takeoff detected
state: WAIT_TARGET
state: WAIT_TARGET
WARN  [mavlink] IGN REQUEST LIST: Busy
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343094.905536831, 15.000000000]: WP: mission received
[ERROR] [1645343094.909636081, 15.004000000]: FCU: IGN REQUEST LIST: Busy
[ERROR] [1645343094.969012656, 15.056000000]: FCU: IGN REQUEST LIST: Busy
[ WARN] [1645343096.506352936, 16.000000000]: RP: timeout, retries left 2
[ WARN] [1645343096.506594236, 16.000000000]: GF: timeout, retries left 2
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343096.507159579, 16.000000000]: RP: mission received
[ERROR] [1645343096.522541863, 16.000000000]: FCU: IGN REQUEST LIST: Busy
state: WAIT_TARGET
[ WARN] [1645343098.068787250, 17.000000000]: GF: timeout, retries left 1
[ INFO] [1645343098.069876863, 17.000000000]: GF: mission received
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
[rvizvisualisation-16] killing on exit
[traj_msg_converter-15] killing on exit
[camera_pose_publisher-14] killing on exit
[rqt_reconfigure-13] killing on exit
[geometric_controller-12] killing on exit
[traj_server-10] killing on exit
[fast_planner_node-9] killing on exit
[mavros-8] killing on exit
[gazebo_gui-6] killing on exit
[waypoint_generator-11] killing on exit
[gazebo-5] killing on exit
Traceback (most recent call last):
  File "/home/njust/catkin_ws/src/px4_fast_planner/scripts/camera_pose_publisher.py", line 41, in <module>
    rate.sleep()
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 103, in sleep
    sleep(self._remaining(curr_time))
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
    raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
[sitl-4] killing on exit

Exiting...
pxh> Shutting down
des manager
[tf_camera-3] killing on exit
[world_map_linker-2] killing on exit
[gazebo_gui-6] escalating to SIGTERM
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

the errors: Warning [parser.cc:950] XML Element[vertical_fov], child of element[camera] not defined in SDF. Ignoring[vertical_fov]. You may have an incorrect SDF file, or an sdformat version that doesn't support this element.

and

WARN  [mavlink] IGN REQUEST LIST: Busy
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343094.905536831, 15.000000000]: WP: mission received
[ERROR] [1645343094.909636081, 15.004000000]: FCU: IGN REQUEST LIST: Busy
[ERROR] [1645343094.969012656, 15.056000000]: FCU: IGN REQUEST LIST: Busy
[ WARN] [1645343096.506352936, 16.000000000]: RP: timeout, retries left 2
[ WARN] [1645343096.506594236, 16.000000000]: GF: timeout, retries left 2
WARN  [mavlink] IGN REQUEST LIST: Busy

I have no idea with these problems. How to solve it? I look forward to your reply. Thank you!

Have you resolved the problem? I meet the problem too.

@yangy-98
Copy link
Author

Hello, when I run the command: roslaunch px4_fast_planner px4_fast_planner.launch
the terminal shows:

... logging to /home/njust/.ros/log/eb340e2c-9220-11ec-b2ca-f8633f2ed425/roslaunch-nuc-17513.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
WARN: unrecognized 'param' tag in <include> tag
started roslaunch server http://nuc:44437/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /camera_pose_publisher/child_frame: camera_link
 * /camera_pose_publisher/parent_frame: world
 * /camera_pose_publisher/pose_topic: camera/pose
 * /fast_planner_node/bspline/limit_acc: 2.5
 * /fast_planner_node/bspline/limit_ratio: 1.1
 * /fast_planner_node/bspline/limit_vel: 1.0
 * /fast_planner_node/fsm/act_map: False
 * /fast_planner_node/fsm/flight_type: 1
 * /fast_planner_node/fsm/thresh_no_replan: 2.0
 * /fast_planner_node/fsm/thresh_replan: 0.5
 * /fast_planner_node/fsm/waypoint0_x: 19.0
 * /fast_planner_node/fsm/waypoint0_y: 0.0
 * /fast_planner_node/fsm/waypoint0_z: 1.0
 * /fast_planner_node/fsm/waypoint1_x: 0.0
 * /fast_planner_node/fsm/waypoint1_y: 10.0
 * /fast_planner_node/fsm/waypoint1_z: 1.0
 * /fast_planner_node/fsm/waypoint2_x: -19.0
 * /fast_planner_node/fsm/waypoint2_y: 0.0
 * /fast_planner_node/fsm/waypoint2_z: 1.0
 * /fast_planner_node/fsm/waypoint_num: 2
 * /fast_planner_node/heading_planner/half_vert_num: 3
 * /fast_planner_node/heading_planner/lambda1: 2.0
 * /fast_planner_node/heading_planner/lambda2: 1.0
 * /fast_planner_node/heading_planner/max_yaw_rate: 1.04719753333
 * /fast_planner_node/heading_planner/w: 10.0
 * /fast_planner_node/heading_planner/yaw_diff: 0.0872664611111
 * /fast_planner_node/manager/clearance_threshold: 0.2
 * /fast_planner_node/manager/control_points_distance: 0.3
 * /fast_planner_node/manager/dynamic_environment: 0
 * /fast_planner_node/manager/local_segment_length: 7.0
 * /fast_planner_node/manager/max_acc: 2.5
 * /fast_planner_node/manager/max_jerk: 4.0
 * /fast_planner_node/manager/max_vel: 1.0
 * /fast_planner_node/manager/use_geometric_path: False
 * /fast_planner_node/manager/use_kinodynamic_path: False
 * /fast_planner_node/manager/use_optimization: True
 * /fast_planner_node/manager/use_topo_path: True
 * /fast_planner_node/optimization/algorithm1: 15
 * /fast_planner_node/optimization/algorithm2: 11
 * /fast_planner_node/optimization/dist0: 0.4
 * /fast_planner_node/optimization/dist1: 0.0
 * /fast_planner_node/optimization/lambda1: 10.0
 * /fast_planner_node/optimization/lambda2: 5.0
 * /fast_planner_node/optimization/lambda3: 0.0
 * /fast_planner_node/optimization/lambda4: 0.001
 * /fast_planner_node/optimization/lambda5: 1.5
 * /fast_planner_node/optimization/lambda6: 10.0
 * /fast_planner_node/optimization/lambda7: 20.0
 * /fast_planner_node/optimization/max_acc: 2.5
 * /fast_planner_node/optimization/max_iteration_num1: 2
 * /fast_planner_node/optimization/max_iteration_num2: 300
 * /fast_planner_node/optimization/max_iteration_time1: 0.0001
 * /fast_planner_node/optimization/max_iteration_time2: 0.005
 * /fast_planner_node/optimization/max_vel: 1.0
 * /fast_planner_node/optimization/order: 3
 * /fast_planner_node/planner_node/planner: 2
 * /fast_planner_node/sdf_map/cx: 320.5
 * /fast_planner_node/sdf_map/cy: 240.5
 * /fast_planner_node/sdf_map/depth_filter_margin: 2
 * /fast_planner_node/sdf_map/depth_filter_maxdist: 7
 * /fast_planner_node/sdf_map/depth_filter_mindist: 0.2
 * /fast_planner_node/sdf_map/depth_filter_tolerance: 0.15
 * /fast_planner_node/sdf_map/esdf_slice_height: 0.3
 * /fast_planner_node/sdf_map/frame_id: world
 * /fast_planner_node/sdf_map/fx: 343.496367536
 * /fast_planner_node/sdf_map/fy: 343.496367536
 * /fast_planner_node/sdf_map/ground_height: -1.0
 * /fast_planner_node/sdf_map/k_depth_scaling_factor: 1000.0
 * /fast_planner_node/sdf_map/local_bound_inflate: 0.5
 * /fast_planner_node/sdf_map/local_map_margin: 50
 * /fast_planner_node/sdf_map/local_update_range_x: 5.5
 * /fast_planner_node/sdf_map/local_update_range_y: 5.5
 * /fast_planner_node/sdf_map/local_update_range_z: 4.5
 * /fast_planner_node/sdf_map/map_size_x: 40.0
 * /fast_planner_node/sdf_map/map_size_y: 40.0
 * /fast_planner_node/sdf_map/map_size_z: 5.0
 * /fast_planner_node/sdf_map/max_ray_length: 4.5
 * /fast_planner_node/sdf_map/min_ray_length: 0.5
 * /fast_planner_node/sdf_map/obstacles_inflation: 0.099
 * /fast_planner_node/sdf_map/p_hit: 0.65
 * /fast_planner_node/sdf_map/p_max: 0.9
 * /fast_planner_node/sdf_map/p_min: 0.12
 * /fast_planner_node/sdf_map/p_miss: 0.35
 * /fast_planner_node/sdf_map/p_occ: 0.8
 * /fast_planner_node/sdf_map/pose_type: 1
 * /fast_planner_node/sdf_map/resolution: 0.1
 * /fast_planner_node/sdf_map/show_esdf_time: False
 * /fast_planner_node/sdf_map/show_occ_time: False
 * /fast_planner_node/sdf_map/skip_pixel: 2
 * /fast_planner_node/sdf_map/use_depth_filter: True
 * /fast_planner_node/sdf_map/virtual_ceil_height: 3.0
 * /fast_planner_node/sdf_map/visualization_truncate_height: 2.5
 * /fast_planner_node/topo_prm/clearance: 0.3
 * /fast_planner_node/topo_prm/max_raw_path2: 25
 * /fast_planner_node/topo_prm/max_raw_path: 300
 * /fast_planner_node/topo_prm/max_sample_num: 2000
 * /fast_planner_node/topo_prm/max_sample_time: 0.005
 * /fast_planner_node/topo_prm/parallel_shortcut: True
 * /fast_planner_node/topo_prm/ratio_to_short: 5.5
 * /fast_planner_node/topo_prm/reserve_num: 6
 * /fast_planner_node/topo_prm/sample_inflate_x: 1.0
 * /fast_planner_node/topo_prm/sample_inflate_y: 5.5
 * /fast_planner_node/topo_prm/sample_inflate_z: 1.0
 * /fast_planner_node/topo_prm/short_cut_num: 1
 * /gazebo/enable_ros_network: True
 * /geometric_controller/Kp_x: 6.0
 * /geometric_controller/Kp_y: 6.0
 * /geometric_controller/Kp_z: 10.0
 * /geometric_controller/Kv_x: 1.5
 * /geometric_controller/Kv_y: 1.5
 * /geometric_controller/Kv_z: 3.5
 * /geometric_controller/ctrl_mode: 2
 * /geometric_controller/enable_gazebo_state: True
 * /geometric_controller/enable_sim: True
 * /geometric_controller/init_pos_x: 0.0
 * /geometric_controller/init_pos_y: 0.0
 * /geometric_controller/init_pos_z: 1.0
 * /geometric_controller/mav_name: iris
 * /geometric_controller/max_acc: 2.5
 * /geometric_controller/velocity_yaw: False
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 408.0
 * /mavros/fake_gps/geo_origin/lat: 47.3667
 * /mavros/fake_gps/geo_origin/lon: 8.55
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /mavros/fake_gps/tf/child_frame_id: fix
 * /mavros/fake_gps/tf/frame_id: map
 * /mavros/fake_gps/tf/listen: False
 * /mavros/fake_gps/tf/rate_limit: 10.0
 * /mavros/fake_gps/tf/send: False
 * /mavros/fake_gps/use_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: udp://:14540@loca...
 * /mavros/gcs_url: 
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/gps_uere: 1.0
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: base_link
 * /mavros/global_position/tf/frame_id: map
 * /mavros/global_position/tf/global_frame_id: earth
 * /mavros/global_position/tf/send: False
 * /mavros/global_position/use_relative_alt: True
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /mavros/imu/frame_id: base_link
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/landing_target/camera/fov_x: 2.0071286398
 * /mavros/landing_target/camera/fov_y: 2.0071286398
 * /mavros/landing_target/image/height: 480
 * /mavros/landing_target/image/width: 640
 * /mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /mavros/landing_target/listen_lt: False
 * /mavros/landing_target/mav_frame: LOCAL_NED
 * /mavros/landing_target/target_size/x: 0.3
 * /mavros/landing_target/target_size/y: 0.3
 * /mavros/landing_target/tf/child_frame_id: camera_center
 * /mavros/landing_target/tf/frame_id: landing_target
 * /mavros/landing_target/tf/listen: False
 * /mavros/landing_target/tf/rate_limit: 10.0
 * /mavros/landing_target/tf/send: True
 * /mavros/local_position/frame_id: map
 * /mavros/local_position/tf/child_frame_id: base_link
 * /mavros/local_position/tf/frame_id: map
 * /mavros/local_position/tf/send: True
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mission/use_mission_item_int: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_thrust: False
 * /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /mavros/setpoint_attitude/tf/frame_id: map
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /mavros/setpoint_attitude/use_quaternion: False
 * /mavros/setpoint_position/mav_frame: LOCAL_NED
 * /mavros/setpoint_position/tf/child_frame_id: target_position
 * /mavros/setpoint_position/tf/frame_id: map
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/setpoint_raw/thrust_scaling: 1.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /mavros/startup_px4_usb_quirk: True
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/time/timesync_mode: MAVLINK
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: odom
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: True
 * /mavros/vision_speed/twist_cov: True
 * /mavros/wheel_odometry/child_frame_id: base_link
 * /mavros/wheel_odometry/count: 2
 * /mavros/wheel_odometry/frame_id: odom
 * /mavros/wheel_odometry/send_raw: True
 * /mavros/wheel_odometry/send_twist: False
 * /mavros/wheel_odometry/tf/child_frame_id: base_link
 * /mavros/wheel_odometry/tf/frame_id: odom
 * /mavros/wheel_odometry/tf/send: False
 * /mavros/wheel_odometry/use_rpm: False
 * /mavros/wheel_odometry/vel_error: 0.1
 * /mavros/wheel_odometry/wheel0/radius: 0.05
 * /mavros/wheel_odometry/wheel0/x: 0.0
 * /mavros/wheel_odometry/wheel0/y: -0.15
 * /mavros/wheel_odometry/wheel1/radius: 0.05
 * /mavros/wheel_odometry/wheel1/x: 0.0
 * /mavros/wheel_odometry/wheel1/y: 0.15
 * /rosdistro: melodic
 * /rosversion: 1.14.12
 * /traj_msg_converter/fast_planner_traj_topic: planning/ref_traj
 * /traj_msg_converter/traj_pub_topic: command/trajectory
 * /traj_server/traj_server/time_forward: 1.5
 * /use_sim_time: True
 * /waypoint_generator/waypoint_type: manual-lonely-way...

NODES
  /
    camera_pose_publisher (px4_fast_planner/camera_pose_publisher.py)
    fast_planner_node (plan_manage/fast_planner_node)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    geometric_controller (geometric_controller/geometric_controller_node)
    mavros (mavros/mavros_node)
    rqt_reconfigure (rqt_reconfigure/rqt_reconfigure)
    rvizvisualisation (rviz/rviz)
    sitl (px4/px4)
    tf_camera (tf/static_transform_publisher)
    traj_msg_converter (px4_fast_planner/trajectory_msg_converter.py)
    traj_server (plan_manage/traj_server)
    vehicle_spawn_nuc_17513_8915702001176790362 (gazebo_ros/spawn_model)
    waypoint_generator (waypoint_generator/waypoint_generator)
    world_map_linker (tf/static_transform_publisher)

auto-starting new master
process[master]: started with pid [17523]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to eb340e2c-9220-11ec-b2ca-f8633f2ed425
process[rosout-1]: started with pid [17534]
started core service [/rosout]
process[world_map_linker-2]: started with pid [17542]
process[tf_camera-3]: started with pid [17543]
process[sitl-4]: started with pid [17554]
INFO  [px4] Creating symlink /home/njust/Firmware/ROMFS/px4fmu_common -> /home/njust/.ros/etc
0 WARNING: setRealtimeSched failed (not run as root?)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
process[gazebo-5]: started with pid [17572]
process[gazebo_gui-6]: started with pid [17581]
process[vehicle_spawn_nuc_17513_8915702001176790362-7]: started with pid [17590]
process[mavros-8]: started with pid [17593]
INFO  [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
INFO  [simulator] Waiting for simulator to connect on TCP port 4560
process[fast_planner_node-9]: started with pid [17606]
process[traj_server-10]: started with pid [17614]
process[waypoint_generator-11]: started with pid [17617]
process[geometric_controller-12]: started with pid [17622]
process[rqt_reconfigure-13]: started with pid [17627]
process[camera_pose_publisher-14]: started with pid [17629]
process[traj_msg_converter-15]: started with pid [17635]
process[rvizvisualisation-16]: started with pid [17637]
[ INFO] [1645343063.946551459]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1645343063.955107417]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1645343063.955237708]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1645343063.955322683]: GCS bridge disabled
[ INFO] [1645343063.978374732]: Plugin 3dr_radio loaded
[ INFO] [1645343063.983463471]: Plugin 3dr_radio initialized
[ INFO] [1645343063.984487249]: Plugin actuator_control loaded
[ INFO] [1645343063.990217028]: Plugin actuator_control initialized
[ INFO] [1645343063.994984539]: Plugin adsb loaded
[ INFO] [1645343064.042044571]: Plugin adsb initialized
[ INFO] [1645343064.042206435]: Plugin altitude loaded
[ INFO] [1645343064.050109989]: Plugin altitude initialized
[ INFO] [1645343064.050250040]: Plugin cam_imu_sync loaded
[ INFO] [1645343064.051451386]: Plugin cam_imu_sync initialized
[ INFO] [1645343064.051579030]: Plugin camera loaded
[ INFO] [1645343064.058256564]: Plugin camera initialized
[ INFO] [1645343064.058408618]: Plugin command loaded
[ INFO] [1645343064.084802218]: Plugin command initialized
[ INFO] [1645343064.084947568]: Plugin companion_process_status loaded
[ INFO] [1645343064.103412163]: Plugin companion_process_status initialized
[ INFO] [1645343064.103553717]: Plugin debug_value loaded
[ INFO] [1645343064.136040285]: Plugin debug_value initialized
[ INFO] [1645343064.136110433]: Plugin distance_sensor blacklisted
[ INFO] [1645343064.136230449]: Plugin esc_status loaded
[ INFO] [1645343064.146330145]: Plugin esc_status initialized
[ INFO] [1645343064.146472963]: Plugin esc_telemetry loaded
[ INFO] [1645343064.147130778]: Plugin esc_telemetry initialized
[ INFO] [1645343064.147274474]: Plugin fake_gps loaded
[ INFO] [1645343064.231376028]: Plugin fake_gps initialized
[ INFO] [1645343064.231514153]: Plugin ftp loaded
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
[ INFO] [1645343064.275973382]: Plugin ftp initialized
[ INFO] [1645343064.276111703]: Plugin geofence loaded
[ INFO] [1645343064.287538938]: Plugin geofence initialized
[ INFO] [1645343064.287688403]: Plugin global_position loaded
[ INFO] [1645343064.361019951]: Plugin global_position initialized
[ INFO] [1645343064.361146509]: Plugin gps_input loaded
[ INFO] [1645343064.364639737]: Plugin gps_input initialized
[ INFO] [1645343064.364820175]: Plugin gps_rtk loaded
[ INFO] [1645343064.367707213]: Plugin gps_rtk initialized
[ INFO] [1645343064.367899555]: Plugin gps_status loaded
[ INFO] [1645343064.377333917]: Plugin gps_status initialized
[ INFO] [1645343064.377554218]: Plugin hil loaded
[ INFO] [1645343064.424574080]: Plugin hil initialized
[ INFO] [1645343064.424801264]: Plugin home_position loaded
[ INFO] [1645343064.431830864]: Plugin home_position initialized
[ INFO] [1645343064.432018631]: Plugin imu loaded
[ INFO] [1645343064.462481289]: Plugin imu initialized
[ INFO] [1645343064.462713493]: Plugin landing_target loaded
[ INFO] [1645343064.501729157]: Plugin landing_target initialized
[ INFO] [1645343064.501953049]: Plugin local_position loaded
[ INFO] [1645343064.517622431]: Plugin local_position initialized
[ INFO] [1645343064.517854420]: Plugin log_transfer loaded
[ INFO] [1645343064.523282175]: Plugin log_transfer initialized
[ INFO] [1645343064.523489691]: Plugin mag_calibration_status loaded
[ INFO] [1645343064.525732422]: Plugin mag_calibration_status initialized
[ INFO] [1645343064.525948884]: Plugin manual_control loaded
[ INFO] [1645343064.531319328]: Plugin manual_control initialized
[ INFO] [1645343064.531539073]: Plugin mocap_pose_estimate loaded
[ INFO] [1645343064.540432768]: Plugin mocap_pose_estimate initialized
[ INFO] [1645343064.540674830]: Plugin mount_control loaded
[ INFO] [1645343064.550353969]: Plugin mount_control initialized
[ INFO] [1645343064.550575753]: Plugin nav_controller_output loaded
[ INFO] [1645343064.551361622]: Plugin nav_controller_output initialized
[ INFO] [1645343064.551554976]: Plugin obstacle_distance loaded
[ INFO] [1645343064.571132641]: Plugin obstacle_distance initialized
[ INFO] [1645343064.571359873]: Plugin odom loaded
[ INFO] [1645343064.586577449]: Plugin odom initialized
[ INFO] [1645343064.586796156]: Plugin onboard_computer_status loaded
[ INFO] [1645343064.590728491]: Plugin onboard_computer_status initialized
[ INFO] [1645343064.590980306]: Plugin param loaded
[ INFO] [1645343064.599312502]: Plugin param initialized
[ INFO] [1645343064.599529226]: Plugin play_tune loaded
[ INFO] [1645343064.604612170]: Plugin play_tune initialized
[ INFO] [1645343064.604794776]: Plugin px4flow loaded
[ INFO] [1645343064.636937334]: Plugin px4flow initialized
[ INFO] [1645343064.637215023]: Plugin rallypoint loaded
[ INFO] [1645343064.647518891]: Plugin rallypoint initialized
[ INFO] [1645343064.647653341]: Plugin rangefinder blacklisted
[ INFO] [1645343064.647835286]: Plugin rc_io loaded
[ INFO] [1645343064.663854075]: Plugin rc_io initialized
[ INFO] [1645343064.663990157]: Plugin safety_area blacklisted
[ INFO] [1645343064.664160325]: Plugin setpoint_accel loaded
[ INFO] [1645343064.677111085]: Plugin setpoint_accel initialized
[ INFO] [1645343064.677364117]: Plugin setpoint_attitude loaded
[ INFO] [1645343064.715683271]: Plugin setpoint_attitude initialized
[ INFO] [1645343064.715868378]: Plugin setpoint_position loaded
INFO  [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0
[ INFO] [1645343064.775354149]: Plugin setpoint_position initialized
[ INFO] [1645343064.775593606]: Plugin setpoint_raw loaded
[ INFO] [1645343064.798244861]: Plugin setpoint_raw initialized
[ INFO] [1645343064.798478187]: Plugin setpoint_trajectory loaded
[ INFO] [1645343064.811059500]: Plugin setpoint_trajectory initialized
[ INFO] [1645343064.811288622]: Plugin setpoint_velocity loaded
[ INFO] [1645343064.833605783]: Plugin setpoint_velocity initialized
[ INFO] [1645343064.833861921]: Plugin sys_status loaded
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 remote port 14550
[ INFO] [1645343064.871425150]: Plugin sys_status initialized
[ INFO] [1645343064.871643251]: Plugin sys_time loaded
[ INFO] [1645343064.902398239]: TM: Timesync mode: MAVLINK
[ INFO] [1645343064.903783475]: TM: Not publishing sim time
[ INFO] [1645343064.908627434]: Plugin sys_time initialized
[ INFO] [1645343064.908845589]: Plugin terrain loaded
[ INFO] [1645343064.910758490]: Plugin terrain initialized
[ INFO] [1645343064.910974435]: Plugin trajectory loaded
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
[ INFO] [1645343064.938611232]: Plugin trajectory initialized
[ INFO] [1645343064.939797177]: Plugin tunnel loaded
[ INFO] [1645343064.950666329]: Plugin tunnel initialized
[ INFO] [1645343064.950836057]: Plugin vfr_hud loaded
[ INFO] [1645343064.954456116]: Plugin vfr_hud initialized
[ INFO] [1645343064.954540368]: Plugin vibration blacklisted
[ INFO] [1645343064.954665211]: Plugin vision_pose_estimate loaded
[ INFO] [1645343065.003085655]: Plugin vision_pose_estimate initialized
[ INFO] [1645343065.003334935]: Plugin vision_speed_estimate loaded
[ INFO] [1645343065.029039283]: Plugin vision_speed_estimate initialized
[ INFO] [1645343065.029300260]: Plugin waypoint loaded
[ INFO] [1645343065.054604623]: Plugin waypoint initialized
[ INFO] [1645343065.054767062]: Plugin wheel_odometry blacklisted
[ INFO] [1645343065.054943486]: Plugin wind_estimation loaded
[ INFO] [1645343065.058992475]: Plugin wind_estimation initialized
[ INFO] [1645343065.059192993]: Autostarting mavlink via USB on PX4
[ INFO] [1645343065.059291293]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1645343065.059370978]: Built-in MAVLink package version: 2022.1.5
[ INFO] [1645343065.059469007]: Known MAVLink dialects: common ardupilotmega ASLUAV all development icarous matrixpilot paparazzi standard uAvionix ualberta
[ INFO] [1645343065.059556398]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1645343065.061561492]: Finished loading Gazebo ROS API Plugin.
INFO  [logger] logger started (mode=all)
[ INFO] [1645343065.067812204]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
INFO  [logger] Start file log (type: full)
INFO  [logger] Opened full log file: ./log/2022-02-20/07_44_25.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
[ INFO] [1645343065.081118935]: udp0: Remote address: 127.0.0.1:14580
[ INFO] [1645343065.082116509]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1645343065.087612519]: FCU: [logger] file: ./log/2022-02-20/07_44_25.ulg
INFO  [px4] Startup script returned successfully
pxh> INFO  [mavlink] partner IP: 127.0.0.1
[ INFO] [1645343065.394119695]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1645343065.397274942]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[INFO] [1645343065.922150, 0.000000]: Loading model XML from file /home/njust/catkin_ws/src/px4_fast_planner/models/iris_depth_camera/iris_depth_camera.sdf
[INFO] [1645343065.924323, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1645343066.097164311]: GF: Using MISSION_ITEM_INT
[ INFO] [1645343066.097215339]: RP: Using MISSION_ITEM_INT
[ INFO] [1645343066.097263950]: WP: Using MISSION_ITEM_INT
[ INFO] [1645343066.097306055]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1645343066.097348038]: VER: 1.1: Flight software:     010a0100 (e0f016c2b3000000)
[ INFO] [1645343066.097376976]: VER: 1.1: Middleware software: 010a0100 (e0f016c2b3000000)
[ INFO] [1645343066.097407808]: VER: 1.1: OS software:         050400ff (427238133be2b0ec)
[ INFO] [1645343066.097440210]: VER: 1.1: Board hardware:      00000001
[ INFO] [1645343066.097544095]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1645343066.097568801]: VER: 1.1: UID:                 4954414c44494e4f
[ INFO] [1645343066.624257870]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1645343066.721117893]: Physics dynamic reconfigure ready.
[INFO] [1645343066.830700, 0.000000]: Calling service /gazebo/spawn_sdf_model
Warning [parser.cc:950] XML Element[vertical_fov], child of element[camera] not defined in SDF. Ignoring[vertical_fov]. You may have an incorrect SDF file, or an sdformat version that doesn't support this element.
[INFO] [1645343067.594226, 0.004000]: Spawn status: SpawnModel: Successfully spawned entity
[vehicle_spawn_nuc_17513_8915702001176790362-7] process has finished cleanly
log file: /home/njust/.ros/log/eb340e2c-9220-11ec-b2ca-f8633f2ed425/vehicle_spawn_nuc_17513_8915702001176790362-7*.log
[ INFO] [1645343068.142457299, 0.004000000]: Camera Plugin: The 'robotNamespace' param did not exit
[ INFO] [1645343068.147006163, 0.004000000]: Camera Plugin (ns = )  <tf_prefix_>, set to ""
INFO  [simulator] Simulator connected on TCP port 4560.
[ INFO] [1645343068.722826320, 0.012000000]: Waiting for home pose...
[ INFO] [1645343068.736453918, 0.024000000]: IMU: High resolution IMU detected!
INFO  [ecl/EKF] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
INFO  [ecl/EKF] 736000: EKF GPS checks passed (WGS-84 origin set)
[ INFO] [1645343069.982568489, 0.940000000]: IMU: Attitude quaternion IMU detected!
[ WARN] [1645343070.053717738, 1.004000000]: [Traj server]: ready.
[FSM]: from INIT to WAIT_TARGET
[ INFO] [1645343070.064806627, 1.012000000]: Home pose initialized to: position: 
  x: 0.000184849
  y: 0.00011755
  z: 0.00167428
orientation: 
  x: 0.0103552
  y: -0.0116411
  z: 0.0191484
  w: -0.999695

[ INFO] [1645343070.633063041, 1.512000000]: Got pose! Drone Ready to be armed.
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
INFO  [ecl/EKF] 5204000: EKF commencing GPS fusion
[ INFO] [1645343078.569473274, 6.004000000]: Offboard enabled
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
[ INFO] [1645343089.590134432, 12.012000000]: FCU: ARMED by Arm/Disarm component command
[ INFO] [1645343089.591626496, 12.012000000]: Vehicle armed
INFO  [commander] Takeoff detected
state: WAIT_TARGET
[ INFO] [1645343089.748329352, 12.060000000]: FCU: Takeoff detected
state: WAIT_TARGET
state: WAIT_TARGET
WARN  [mavlink] IGN REQUEST LIST: Busy
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343094.905536831, 15.000000000]: WP: mission received
[ERROR] [1645343094.909636081, 15.004000000]: FCU: IGN REQUEST LIST: Busy
[ERROR] [1645343094.969012656, 15.056000000]: FCU: IGN REQUEST LIST: Busy
[ WARN] [1645343096.506352936, 16.000000000]: RP: timeout, retries left 2
[ WARN] [1645343096.506594236, 16.000000000]: GF: timeout, retries left 2
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343096.507159579, 16.000000000]: RP: mission received
[ERROR] [1645343096.522541863, 16.000000000]: FCU: IGN REQUEST LIST: Busy
state: WAIT_TARGET
[ WARN] [1645343098.068787250, 17.000000000]: GF: timeout, retries left 1
[ INFO] [1645343098.069876863, 17.000000000]: GF: mission received
state: WAIT_TARGET
state: WAIT_TARGET
state: WAIT_TARGET
[rvizvisualisation-16] killing on exit
[traj_msg_converter-15] killing on exit
[camera_pose_publisher-14] killing on exit
[rqt_reconfigure-13] killing on exit
[geometric_controller-12] killing on exit
[traj_server-10] killing on exit
[fast_planner_node-9] killing on exit
[mavros-8] killing on exit
[gazebo_gui-6] killing on exit
[waypoint_generator-11] killing on exit
[gazebo-5] killing on exit
Traceback (most recent call last):
  File "/home/njust/catkin_ws/src/px4_fast_planner/scripts/camera_pose_publisher.py", line 41, in <module>
    rate.sleep()
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 103, in sleep
    sleep(self._remaining(curr_time))
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
    raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
[sitl-4] killing on exit

Exiting...
pxh> Shutting down
des manager
[tf_camera-3] killing on exit
[world_map_linker-2] killing on exit
[gazebo_gui-6] escalating to SIGTERM
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

the errors: Warning [parser.cc:950] XML Element[vertical_fov], child of element[camera] not defined in SDF. Ignoring[vertical_fov]. You may have an incorrect SDF file, or an sdformat version that doesn't support this element.
and

WARN  [mavlink] IGN REQUEST LIST: Busy
WARN  [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1645343094.905536831, 15.000000000]: WP: mission received
[ERROR] [1645343094.909636081, 15.004000000]: FCU: IGN REQUEST LIST: Busy
[ERROR] [1645343094.969012656, 15.056000000]: FCU: IGN REQUEST LIST: Busy
[ WARN] [1645343096.506352936, 16.000000000]: RP: timeout, retries left 2
[ WARN] [1645343096.506594236, 16.000000000]: GF: timeout, retries left 2
WARN  [mavlink] IGN REQUEST LIST: Busy

I have no idea with these problems. How to solve it? I look forward to your reply. Thank you!

Have you resolved the problem? I meet the problem too.

I'm sorry, I haven't solved the problem.

@Cavalletta98
Copy link

Have someone solved it?

@Bugday001
Copy link

Bugday001 commented Jan 22, 2023 •
edited
Loading

#24 may solve this problem.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
4 participants
@Cavalletta98 @yangy-98 @calmelo @Bugday001