robin-shaun / XTDrone

UAV Simulation Platform based on PX4, ROS and Gazebo
MIT License
1.11k stars 204 forks source link

您好!运行VINS-fusion时,仿真环境飞机图像和imu话题都有数据,但是vins-fusion相关的话题都没有数据 #103

Open SSZ1 opened 5 months ago

SSZ1 commented 5 months ago

第一个问题:运行indoor1.launch后i,/iris_0/stereo_camera/right/image_raw、/iris_0/stereo_camera/right/image_raw、/iris_0/imu_gazebo三个话题都有数据, 但是vins相关的话题,/vins_estimator/image_track、/vins_estimator/camera_pose都没有数据输出,rviz图像位置不显示,EKF2已修改为视觉导航模式,因为vins无法输出位姿,因此解锁飞机时没有反应。请问这是什么问题? 以下是相关消息数据: image image image

image

仿真环境与vins运行窗口为 image

`ssz@ssz-G5-5500:~/WorkSpace/Auto-Uav/PX4_Firmware$ roslaunch px4 indoor1.launch ... logging to /home/ssz/.ros/log/b94d2e08-eb23-11ee-8e4a-f5e9abdcc60b/roslaunch-ssz-G5-5500-3418492.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.

started roslaunch server http://ssz-G5-5500:44051/

SUMMARY

CLEAR PARAMETERS

PARAMETERS

NODES / gazebo (gazebo_ros/gzserver) gazebo_gui (gazebo_ros/gzclient) /iris_0/ iris_0_spawn (gazebo_ros/spawn_model) mavros (mavros/mavros_node) sitl_0 (px4/px4)

ROS_MASTER_URI=http://localhost:11311

process[gazebo-1]: started with pid [3418523] process[gazebo_gui-2]: started with pid [3418528] process[iris_0/sitl_0-3]: started with pid [3418537] Creating symlink /home/ssz/WorkSpace/Auto-Uav/PX4_Firmware/build/px4_sitl_default/etc -> /home/ssz/.ros/sitl_iris_0/etc


| \ \ \ / / / | | |/ / \ V / / /| | | / / \ / /_| | | | / /^\ \ __ | _| \/ \/ |/

px4 starting.

INFO [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0 INFO [init] found model autostart file as SYS_AUTOSTART=10016 INFO [param] selected parameter default file eeprom/parameters_10016 process[iris_0/iris_0_spawn-4]: started with pid [3418553] INFO [parameters] BSON document size 349 bytes, decoded 349 bytes (INT32:15, FLOAT:3) [param] Loaded: eeprom/parameters_10016 process[iris_0/mavros-5]: started with pid [3418561] [ INFO] [1711429132.069247521]: FCU URL: udp://:24540@localhost:34580 [ INFO] [1711429132.072063779]: udp0: Bind address: 0.0.0.0:24540 [ INFO] [1711429132.072174329]: udp0: Remote address: 127.0.0.1:34580 [ INFO] [1711429132.072236104]: GCS bridge disabled [ INFO] [1711429132.081737972]: Plugin 3dr_radio loaded INFO [dataman] data manager file './dataman' size is 7866640 bytes [ INFO] [1711429132.083073863]: Plugin 3dr_radio initialized [ INFO] [1711429132.083148962]: Plugin actuator_control loaded [ INFO] [1711429132.085396436]: Plugin actuator_control initialized PX4 SIM HOST: localhost [ INFO] [1711429132.089578522]: Plugin adsb loaded [ INFO] [1711429132.092016168]: Plugin adsb initialized [ INFO] [1711429132.092130358]: Plugin altitude loaded [ INFO] [1711429132.092824377]: Plugin altitude initialized [ INFO] [1711429132.092905993]: Plugin cam_imu_sync loaded [ INFO] [1711429132.093316961]: Plugin cam_imu_sync initialized [ INFO] [1711429132.093394335]: Plugin camera loaded [ INFO] [1711429132.093811183]: Plugin camera initialized [ INFO] [1711429132.093894252]: Plugin cellular_status loaded [ INFO] [1711429132.095683555]: Plugin cellular_status initialized [ INFO] [1711429132.095778258]: Plugin command loaded INFO [simulator] Waiting for simulator to accept connection on TCP port 4560 [ INFO] [1711429132.100949465]: Plugin command initialized [ INFO] [1711429132.101067646]: Plugin companion_process_status loaded [ INFO] [1711429132.102903007]: Plugin companion_process_status initialized [ INFO] [1711429132.103003855]: Plugin debug_value loaded [ INFO] [1711429132.106784696]: Plugin debug_value initialized [ INFO] [1711429132.106820189]: Plugin distance_sensor blacklisted [ INFO] [1711429132.106919988]: Plugin esc_status loaded [ INFO] [1711429132.107732109]: Plugin esc_status initialized [ INFO] [1711429132.107826142]: Plugin esc_telemetry loaded [ INFO] [1711429132.108247497]: Plugin esc_telemetry initialized [ INFO] [1711429132.108341665]: Plugin fake_gps loaded [ INFO] [1711429132.119753697]: Plugin fake_gps initialized [ INFO] [1711429132.119922851]: Plugin ftp loaded [ INFO] [1711429132.125269589]: Plugin ftp initialized [ INFO] [1711429132.125467582]: Plugin geofence loaded [ INFO] [1711429132.127801069]: Plugin geofence initialized [ INFO] [1711429132.127947469]: Plugin global_position loaded [ INFO] [1711429132.140098902]: Plugin global_position initialized [ INFO] [1711429132.140213564]: Plugin gps_input loaded [ INFO] [1711429132.142468327]: Plugin gps_input initialized [ INFO] [1711429132.142553601]: Plugin gps_rtk loaded [ INFO] [1711429132.144663489]: Plugin gps_rtk initialized [ INFO] [1711429132.144748032]: Plugin gps_status loaded [ INFO] [1711429132.146381321]: Plugin gps_status initialized [ INFO] [1711429132.146470232]: Plugin guided_target loaded [ INFO] [1711429132.149629322]: Plugin guided_target initialized [ INFO] [1711429132.149725154]: Plugin hil loaded [ INFO] [1711429132.158979252]: Plugin hil initialized [ INFO] [1711429132.159108784]: Plugin home_position loaded [ INFO] [1711429132.161673795]: Plugin home_position initialized [ INFO] [1711429132.161772442]: Plugin imu loaded [ INFO] [1711429132.165715977]: Plugin imu initialized [ INFO] [1711429132.165790995]: Plugin landing_target loaded [ INFO] [1711429132.173684281]: Plugin landing_target initialized [ INFO] [1711429132.173790866]: Plugin local_position loaded [ INFO] [1711429132.177232176]: Plugin local_position initialized [ INFO] [1711429132.177314186]: Plugin log_transfer loaded [ INFO] [1711429132.178958080]: Plugin log_transfer initialized [ INFO] [1711429132.179022549]: Plugin mag_calibration_status loaded [ INFO] [1711429132.179601937]: Plugin mag_calibration_status initialized [ INFO] [1711429132.179667319]: Plugin manual_control loaded [ INFO] [1711429132.181150360]: Plugin manual_control initialized [ INFO] [1711429132.181203092]: Plugin mocap_pose_estimate loaded [ INFO] [1711429132.183372085]: Plugin mocap_pose_estimate initialized [ INFO] [1711429132.183445511]: Plugin mount_control loaded [ WARN] [1711429132.186263525]: Could not retrive negate_measured_roll parameter value, using default (0) [ WARN] [1711429132.186485388]: Could not retrive negate_measured_pitch parameter value, using default (0) [ WARN] [1711429132.186694728]: Could not retrive negate_measured_yaw parameter value, using default (0) [ WARN] [1711429132.187475621]: Could not retrive debounce_s parameter value, using default (4.000000) [ WARN] [1711429132.187687880]: Could not retrive err_threshold_deg parameter value, using default (10.000000) [ INFO] [1711429132.187733250]: Plugin mount_control initialized [ INFO] [1711429132.187822495]: Plugin nav_controller_output loaded [ INFO] [1711429132.188359285]: Plugin nav_controller_output initialized [ INFO] [1711429132.188449102]: Plugin obstacle_distance loaded [ INFO] [1711429132.190214558]: Plugin obstacle_distance initialized [ INFO] [1711429132.190358627]: Plugin odom loaded [ INFO] [1711429132.193300003]: Plugin odom initialized [ INFO] [1711429132.193418449]: Plugin onboard_computer_status loaded [ INFO] [1711429132.195062195]: Plugin onboard_computer_status initialized [ INFO] [1711429132.195164517]: Plugin param loaded [ INFO] [1711429132.197221235]: Plugin param initialized [ INFO] [1711429132.197308293]: Plugin play_tune loaded [ INFO] [1711429132.199111556]: Plugin play_tune initialized [ INFO] [1711429132.199196308]: Plugin px4flow loaded [ INFO] [1711429132.204380954]: Plugin px4flow initialized [ INFO] [1711429132.204482010]: Plugin rallypoint loaded [ INFO] [1711429132.206574502]: Plugin rallypoint initialized [ INFO] [1711429132.206604836]: Plugin rangefinder blacklisted [ INFO] [1711429132.206678453]: Plugin rc_io loaded [ INFO] [1711429132.209075968]: Plugin rc_io initialized [ INFO] [1711429132.209105533]: Plugin safety_area blacklisted [ INFO] [1711429132.209174919]: Plugin setpoint_accel loaded [ INFO] [1711429132.211074662]: Plugin setpoint_accel initialized [ INFO] [1711429132.211199330]: Plugin setpoint_attitude loaded [ INFO] [1711429132.216365076]: Plugin setpoint_attitude initialized [ INFO] [1711429132.216444185]: Plugin setpoint_position loaded [ INFO] [1711429132.224841652]: Plugin setpoint_position initialized [ INFO] [1711429132.224929652]: Plugin setpoint_raw loaded [ INFO] [1711429132.229904249]: Plugin setpoint_raw initialized [ INFO] [1711429132.230022385]: Plugin setpoint_trajectory loaded [ INFO] [1711429132.232569903]: Plugin setpoint_trajectory initialized [ INFO] [1711429132.232681107]: Plugin setpoint_velocity loaded [ INFO] [1711429132.235582430]: Plugin setpoint_velocity initialized [ INFO] [1711429132.235718537]: Plugin sys_status loaded [ INFO] [1711429132.241171963]: Plugin sys_status initialized [ INFO] [1711429132.241266883]: Plugin sys_time loaded [ INFO] [1711429132.244230564]: TM: Timesync mode: MAVLINK [ INFO] [1711429132.244434953]: TM: Not publishing sim time [ INFO] [1711429132.245028304]: Plugin sys_time initialized [ INFO] [1711429132.245092239]: Plugin terrain loaded [ INFO] [1711429132.245381556]: Plugin terrain initialized [ INFO] [1711429132.245437228]: Plugin trajectory loaded [ INFO] [1711429132.248073583]: Plugin trajectory initialized [ INFO] [1711429132.248140975]: Plugin tunnel loaded [ INFO] [1711429132.249577455]: Plugin tunnel initialized [ INFO] [1711429132.249647710]: Plugin vfr_hud loaded [ INFO] [1711429132.249948943]: Plugin vfr_hud initialized [ INFO] [1711429132.249968246]: Plugin vibration blacklisted [ INFO] [1711429132.250022907]: Plugin vision_pose_estimate loaded [ INFO] [1711429132.254083638]: Plugin vision_pose_estimate initialized [ INFO] [1711429132.254180424]: Plugin vision_speed_estimate loaded [ INFO] [1711429132.256317692]: Plugin vision_speed_estimate initialized [ INFO] [1711429132.256524099]: Plugin waypoint loaded [ INFO] [1711429132.259395098]: Plugin waypoint initialized [ INFO] [1711429132.259438714]: Plugin wheel_odometry blacklisted [ INFO] [1711429132.259552898]: Plugin wind_estimation loaded [ INFO] [1711429132.259878260]: Plugin wind_estimation initialized [ INFO] [1711429132.259980837]: Built-in SIMD instructions: SSE, SSE2 [ INFO] [1711429132.260010421]: Built-in MAVLink package version: 2024.3.3 [ INFO] [1711429132.260040409]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all csAirLink cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta [ INFO] [1711429132.260066544]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [INFO] [1711429132.382758, 0.000000]: Loading model XML from ros parameter model_description [INFO] [1711429132.388242, 0.000000]: Waiting for service /gazebo/spawn_sdf_model [ INFO] [1711429132.519168085]: Finished loading Gazebo ROS API Plugin. [ INFO] [1711429132.520087413]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting... [ INFO] [1711429132.602655696]: Finished loading Gazebo ROS API Plugin. [ INFO] [1711429132.604162326]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting... [ WARN] [1711429132.675978313, 5947.033000000]: Shutdown request received. [ WARN] [1711429132.676085076, 5947.033000000]: Reason given for shutdown: [[/gazebo_gui] Reason: new node registered with same name] [ INFO] [1711429132.971313523]: waitForService: Service [/gazebo/set_physics_properties] is now available. [INFO] [1711429132.992726, 0.000000]: Calling service /gazebo/spawn_sdf_model [ INFO] [1711429133.001951894]: Physics dynamic reconfigure ready. Trace/breakpoint trap (core dumped) [INFO] [1711429133.033231, 1814.801000]: Spawn status: SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name iris_0 [ERROR] [1711429133.034560, 1814.801000]: Spawn service failed. Exiting. Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring. Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring. Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring. Warning [parser.cc:833] XML Attribute[version] in element[sdf] not defined in SDF, ignoring. [iris_0/iris_0_spawn-4] process has died [pid 3418553, exit code 1, cmd /home/ssz/WorkSpace/Auto-Uav/uav_sim/devel/lib/gazebo_ros/spawn_model -sdf -param model_description -model iris_0 -x 0 -y 7.5 -z 0.5 -R 0 -P 0 -Y 0 name:=iris_0_spawn log:=/home/ssz/.ros/log/b94d2e08-eb23-11ee-8e4a-f5e9abdcc60b/iris_0-iris_0_spawn-4.log]. log file: /home/ssz/.ros/log/b94d2e08-eb23-11ee-8e4a-f5e9abdcc60b/iris_0-iris_0_spawn-4*.log [ INFO] [1711429134.868184157, 1814.857000000]: Camera Plugin: The 'robotNamespace' param was empty [ INFO] [1711429134.868710812, 1814.857000000]: Camera Plugin: The 'robotNamespace' param was empty [ INFO] [1711429134.871037654, 1814.857000000]: Camera Plugin (ns = iris_0) , set to "iris_0" [ INFO] [1711429134.871107412, 1814.857000000]: Camera Plugin (ns = iris_0) , set to "iris_0" [ INFO] [1711429134.878689616, 1814.857000000]: is unset, using default value of false to comply with REP 145 (world as orientation reference) [ WARN] [1711429134.878761318, 1814.857000000]: missing , set to default: /iris_0/ [ INFO] [1711429134.878792294, 1814.857000000]: set to: imu_gazebo [ INFO] [1711429134.878809645, 1814.857000000]: set to: imu_link_stereo [ INFO] [1711429134.878842927, 1814.857000000]: set to: 500 [ INFO] [1711429134.878875166, 1814.857000000]: set to: 0 [ INFO] [1711429134.878913057, 1814.857000000]: set to: 0 0 0 [ INFO] [1711429134.878960517, 1814.857000000]: set to: 0 -0 0 [ INFO] [1711429136.460509552, 1814.857000000]: bumper plugin missing , defaults to world INFO [simulator] Simulator connected on TCP port 4560. INFO [commander] LED: open /dev/led0 failed (22) INFO [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0 INFO [init] setting PWM_AUX_OUT none INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550 INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 34580 remote port 24540 INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030 INFO [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280 [ INFO] [1711429136.833180672, 1815.125000000]: udp0: Remote address: 127.0.0.1:34580 [ INFO] [1711429136.833380357, 1815.125000000]: IMU: High resolution IMU detected! INFO [mavlink] partner IP: 127.0.0.1 INFO [logger] logger started (mode=all) INFO [logger] Start file log (type: full) INFO [logger] [logger] ./log/2024-03-26/04_58_56.ulg
INFO [logger] Opened full log file: ./log/2024-03-26/04_5856.ulg INFO [mavlink] MAVLink only on localhost (set param MAV{i}BROADCAST = 1 to enable network) INFO [mavlink] MAVLink only on localhost (set param MAV{i}_BROADCAST = 1 to enable network) INFO [px4] Startup script returned successfully INFO [mavlink] partner IP: 127.0.0.1 [ INFO] [1711429137.820706743, 1816.105000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot [ INFO] [1711429137.821932180, 1816.105000000]: IMU: High resolution IMU detected! [ INFO] [1711429138.443485212, 1816.721000000]: IMU: Attitude quaternion IMU detected! [ INFO] [1711429138.831757039, 1817.109000000]: GF: Using MISSION_ITEM_INT [ INFO] [1711429138.832217439, 1817.109000000]: RP: Using MISSION_ITEM_INT [ INFO] [1711429138.832456838, 1817.109000000]: WP: Using MISSION_ITEM_INT [ INFO] [1711429138.832803037, 1817.109000000]: VER: 1.1: Capabilities 0x000000000000e4ff [ INFO] [1711429138.833412898, 1817.109000000]: VER: 1.1: Flight software: 010d0200 (46a12a09bf000000) [ INFO] [1711429138.833748243, 1817.109000000]: VER: 1.1: Middleware software: 010d0200 (46a12a09bf000000) [ INFO] [1711429138.833889119, 1817.109000000]: VER: 1.1: OS software: 050f00ff (91bece51afbe7da9) [ INFO] [1711429138.834028074, 1817.109000000]: VER: 1.1: Board hardware: 00000001 [ INFO] [1711429138.834201315, 1817.109000000]: VER: 1.1: VID/PID: 0000:0000 [ INFO] [1711429138.834308221, 1817.109000000]: VER: 1.1: UID: 4954414c44494e4f [ WARN] [1711429138.836554245, 1817.113000000]: CMD: Unexpected command 520, result 0 INFO [tone_alarm] home set INFO [tone_alarm] notify negative [ INFO] [1711429152.862584552, 1831.105000000]: GF: mission received [ INFO] [1711429152.862730047, 1831.105000000]: RP: mission received [ INFO] [1711429152.862784508, 1831.105000000]: WP: mission received

`

里面有一个报错, [iris_0/iris_0_spawn-4] process has died [pid 3418553, exit code 1, cmd /home/ssz/WorkSpace/Auto-Uav/uav_sim/devel/lib/gazebo_ros/spawn_model -sdf -param model_description -model iris_0 -x 0 -y 7.5 -z 0.5 -R 0 -P 0 -Y 0 __name:=iris_0_spawn __log:=/home/ssz/.ros/log/b94d2e08-eb23-11ee-8e4a-f5e9abdcc60b/iris_0-iris_0_spawn-4.log]. log file: /home/ssz/.ros/log/b94d2e08-eb23-11ee-8e4a-f5e9abdcc60b/iris_0-iris_0_spawn-4*.log

`ssz@ssz-G5-5500:~/WorkSpace/Auto-Uav/uav_sim$ bash scripts/xtdrone_run_vio.sh [ INFO] [1711424936.583679628]: init begins config_file: /home/ssz/WorkSpace/Auto-Uav/uav_sim/src/VINS-Fusion/config/xtdrone_sitl/px4_sitl_stereo_imu_config.yaml USE_IMU: 1 IMU_TOPIC: /iris_0/imu_gazebo result path /home/ssz/WorkSpace/Auto-Uav/uav_sim/vins_output/vio.csv [ WARN] [1711424936.616103346]: fix extrinsic param camera number 2 [ INFO] [1711424936.619258060]: Synchronized sensors, fix time offset: 0 [ INFO] [1711424936.619312802]: ROW: 480 COL: 752 exitrinsic cam 0 0 0 1 -1 0 0 0 -1 0 0 0.12 -0.3 exitrinsic cam 1 0 0 1 -1 0 0 0 -1 0 0 0 -0.3 set g 0 0 9.81007 [ INFO] [1711424936.619430516]: reading paramerter of camera /home/ssz/WorkSpace/Auto-Uav/uav_sim/src/VINS-Fusion/config/xtdrone_sitl/cam0_pinhole_p1.yaml [ INFO] [1711424936.619646728]: reading paramerter of camera /home/ssz/WorkSpace/Auto-Uav/uav_sim/src/VINS-Fusion/config/xtdrone_sitl/cam1_pinhole_p1.yaml MULTIPLE_THREAD is 1 [ WARN] [1711424936.619819237]: waiting for image and imu... ... logging to /home/ssz/.ros/log/b94d2e08-eb23-11ee-8e4a-f5e9abdcc60b/roslaunch-ssz-G5-5500-3392020.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.

started roslaunch server http://ssz-G5-5500:39389/

SUMMARY

PARAMETERS

NODES / rvizvisualisation (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[rvizvisualisation-1]: started with pid [3392081] `

第二个问题:ekf2为gps和气压计模式时,运行indoor1.launch,使用键盘控制飞机,解锁后,飞机会出现乱飞的情况,请问这是什么原因

l-curious commented 4 months ago

解决了吗,我vins-fusion 和 三维激光都没rviz显示,二位激光可以

l-curious commented 4 months ago

是不是ceres和eigen3兼容的问题,

2874160799 commented 4 months ago

是不是ceres和eigen3兼容的问题,

是ceres1.14不行还是eigen3不行呢

l-curious commented 4 months ago

我的是 将如下代码添加到/usr/local/lib/cmake/Ceres/CeresConfig.cmake中: set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") #添加此行 set(CERES_EIGEN_VERSION 3.3.9) #添加此行,版本号与你安装的对应

a0c8a041ab204da2a7adf21a623c562b

https://blog.csdn.net/weixin_48093237/article/details/138862043?spm=1001.2014.3001.5501

2874160799 commented 4 months ago

我的是 将如下代码添加到/usr/local/lib/cmake/Ceres/CeresConfig.cmake中: set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") #添加此行 set(CERES_EIGEN_VERSION 3.3.9) #添加此行,版本号与你安装的对应

a0c8a041ab204da2a7adf21a623c562b

https://blog.csdn.net/weixin_48093237/article/details/138862043?spm=1001.2014.3001.5501

您好,按照这篇博客的方法,rviz出现了轨迹,但是点云还是没有,不知道是哪里还有点问题 2

l-curious commented 4 months ago

这个我还真没遇到过,要不试试双系统