Hi @szebedy!

Can you have a problem with scale in odometry?
I try to use stereo cam RealSense with high accuracy IMU in SVO 2.0 odometry...
When I cyclically shift a drone between two stationary points A(x1,y1,z1) and B(x2,y1,z1), drone in any iterations flies in new points with low, and low, and low scale

May be you know some parameters in config files to solve this problem?

Some of parameter in my yaml file

pipeline_is_stereo: True
automatic_reinitialization: True # When lost, stereo can recover immediately
grid_size: 35
use_async_reprojectors: True
use_imu: True
poseoptim_prior_lambda: 2.0
img_align_prior_lambda_rot: 5.0
img_align_prior_lambda_trans: 0.0

use_threaded_depthfilter: True
quality_min_fts: 40
quality_max_drop_fts: 80

max_depth_inv: 0.1
min_depth_inv: 10.0
mean_depth_inv: 0.5

map_scale: 1.0
kfselect_criterion: DOWNLOOKING
kfselect_numkfs_upper_thresh: 180
kfselect_numkfs_lower_thresh: 90
kfselect_min_dist_metric: 0.001
kfselect_min_angle: 6
kfselect_min_disparity: 40
kfselect_min_num_frames_between_kfs: 0

img_align_est_illumination_gain: true
img_align_est_illumination_offset: true
depth_filter_affine_est_offset: true
depth_filter_affine_est_gain: true
reprojector_affine_est_offset: true
reprojector_affine_est_gain: true

[Msg] Waiting for master.
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.0.2.15
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.0.2.15
[Wrn] [msgs.cc:1655] Conversion of sensor type[depth] not suppported.
[Err] [Plugin.hh:165] Failed to load plugin libgazebo_ros_camera.so: libCameraPlugin.so: cannot open shared object file: No such file or directory
[Err] [Plugin.hh:165] Failed to load plugin libgazebo_ros_openni_kinect.so: libDepthCameraPlugin.so: cannot open shared object file: No such file or directory
[Err] [Distortion.cc:63] Pincushion model is currently not supported. Please use a negative k1 coefficient for barrel distortion
[Err] [Distortion.cc:80] Currently only Barrel Distortion is supported. Distortion will not be applied.
[Err] [Plugin.hh:165] Failed to load plugin libgazebo_ros_camera.so: libCameraPlugin.so: cannot open shared object file: No such file or directory
[Dbg] [gazebo_mavlink_interface.cpp:133] <joint_name> not found for channel[0] no joint control will be performed for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:133] <joint_name> not found for channel[1] no joint control will be performed for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:133] <joint_name> not found for channel[2] no joint control will be performed for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:133] <joint_name> not found for channel[3] no joint control will be performed for this channel.
[Wrn] [gazebo_mavlink_interface.cpp:122] joint [zephyr_delta_wing::propeller_joint] not found for channel[4] no joint control for this channel.
[Wrn] [gazebo_mavlink_interface.cpp:122] joint [zephyr_delta_wing::flap_left_joint] not found for channel[5] no joint control for this channel.
[Wrn] [gazebo_mavlink_interface.cpp:122] joint [zephyr_delta_wing::flap_right_joint] not found for channel[6] no joint control for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:133] <joint_name> not found for channel[7] no joint control will be performed for this channel.
[Wrn] [msgs.cc:1655] Conversion of sensor type[depth] not suppported.
[Wrn] [Publisher.cc:140] Queue limit reached for topic /gazebo/default/iris_r200/motor_speed/0, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:140] Queue limit reached for topic /gazebo/default/iris_r200/motor_speed/1, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:140] Queue limit reached for topic /gazebo/default/iris_r200/motor_speed/2, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:140] Queue limit reached for topic /gazebo/default/iris_r200/motor_speed/3, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:140] Queue limit reached for topic /gazebo/default/iris_r200/imu, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:140] Queue limit reached for topic /gazebo/default/iris_r200/motors, deleting message. This warning is printed only once.
INFO [simulator] Got initial simulation data, running sim..
INFO [pwm_out_sim] MODE_16PWM
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14556 remote port 14550
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14557 remote port 14540
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
pxh> INFO [logger] logger started (mode=all)
INFO [logger] Start file log
INFO [logger] Opened log file: rootfs/fs/microsd/log/2019-01-30/07_35_16.ulg
INFO [Unknown] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
gzserver: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreRenderSystem.cpp:546: virtual void Ogre::RenderSystem::setDepthBufferFor(Ogre::RenderTarget*): Assertion `bAttached && "A new DepthBuffer for a RenderTarget was created, but after creation" "it says it's incompatible with that RT"' failed.

Hi @szebedy sorry to disturb.
I tried to install your project on my computer. (Linux 16.04)
when I run the command you write on the introduction it will show this error:

zxp@zxp-mint ~/catkin_ws $ git submodule update --init --recursive
zxp@zxp-mint ~/catkin_ws $ cd ~/catkin_ws/px4
zxp@zxp-mint ~/catkin_ws/px4 $ make posix_sitl_default gazebo
Makefile:39: *** YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING... Stop.
could you give me some advice to figure out the error?

HI Szebedy:
Thank you for your sharing, but when I want to download the SVO 2.0 package from http://rpg.ifi.uzh.ch/svo2download.html, it is empty, do you know why this is? And do you know where I could download SVO 2.0 package now?

hi,
During compilation, the package creates a compile problem. How can I solve this?

Please, tel how to accses gitlab.ethz.ch?
git clone https://gitlab.ethz.ch/disco-students/fs18/bszebedy.git catkin_ws
Username for 'https://gitlab.ethz.ch':
Password for 'https://gitlab.ethz.ch':

When compiling usb_cam, I have encountered the following problem:

ld: error: /home/he/catkin_ws/devel/.private/usb_cam/lib/libusb_cam.so: undefined reference to av_frame_alloc
ld: error: /home/he/catkin_ws/devel/.private/usb_cam/lib/libusb_cam.so: undefined reference to av_free
clang: error: linker command failed with exit code 1 (use -v to see invocation)
make[2]: *** [/home/he/catkin_ws/devel/.private/usb_cam/lib/usb_cam/usb_cam_node] Error 1
make[1]: *** [CMakeFiles/usb_cam_node.dir/all] Error 2
make: *** [all] Error 2
cd /home/he/catkin_ws/build/usb_cam; catkin build --get-env usb_cam | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

After checking related issues on ros_driver/usb_cam, I still get stuck on it. Therefore, I would like to seek advice from you. Thanks in advance!

Hello! @szebedy I think the 3D model in the Gazebo is not the intel drone's model. Maybe this is ok for simulation. But I am wondering whether this would cause some problem when applying the code in real intel drone in the real world? I am also wondering why you didn't use the intel 3D model? Thank you!

Hi @szebedy ,

first of all thanks for sharing your code!
I have installed Ubuntu 16.04 and installed ROS according to your explanations. I can also both prepare the tools for building PX4 and download the source files from your account.
However, when trying to update the submodules, I get the following error (I have entered the command twice, so that is why here you cannot see all the submodules that worked):

This leaves me behind with an empty folder “~/catkin_ws/px4/src/modules/uavcan/libuavcan”. It seems that I am not able to use the commit e1c9a4f5064a336295125da25c0542a5cd601dd7, but the most recent one (i.e. 67da0aae7dd4eedae7905ab98941592cebab4723) works fine if I simply clone it with “git clone https://github.com/UAVCAN/libuavcan” in ~/catkin_ws/px4/src/modules/uavcan. This is seen here:

However, the next line does not work then. This fatal error occurs:

I assume your code expects exactly the commit which is not working for me? Unfortunately, I have not found the answer in any other forum yet. Do you have any suggestions?

Thanks in advance!

Hi, I'm sorry that I have some boring questions about the off_board module. In the module, I can only find the ROS Client, but I can't find the server, where could I find the server?

Hi!@szebedy
When I want to do the simulation and do this in the teminal 1:

cd ~/svo_ws/
source ./devel/setup.bash
roslaunch svo_ros intel_aero.launch

It shows that:

SUMMARY
========

CLEAR PARAMETERS
 * /svo/

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /svo/T_world_imuinit/qw: 1
 * /svo/T_world_imuinit/qx: 0
 * /svo/T_world_imuinit/qy: 0
 * /svo/T_world_imuinit/qz: 0
 * /svo/calib_file: /home/fanjiaojiao...
 * /svo/cam0_topic: /camera/mono/imag...
 * /svo/depth_filter_affine_est_gain: True
 * /svo/depth_filter_affine_est_offset: True
 * /svo/detector_threshold_primary: 10
 * /svo/detector_threshold_secondary: 200
 * /svo/grid_size: 35
 * /svo/img_align_est_illumination_gain: True
 * /svo/img_align_est_illumination_offset: True
 * /svo/img_align_max_level: 4
 * /svo/img_align_min_level: 2
 * /svo/img_align_prior_lambda_rot: 0.0
 * /svo/img_align_prior_lambda_trans: 0.0
 * /svo/imu_topic: /mavros/imu/data
 * /svo/init_min_disparity: 30
 * /svo/kfselect_criterion: FORWARD
 * /svo/kfselect_min_angle: 6
 * /svo/kfselect_min_disparity: 40
 * /svo/kfselect_min_dist_metric: 0.001
 * /svo/kfselect_numkfs_lower_thresh: 70
 * /svo/kfselect_numkfs_upper_thresh: 180
 * /svo/map_scale: 1.5
 * /svo/max_depth_inv: 0.05
 * /svo/max_fts: 180
 * /svo/max_n_kfs: 30
 * /svo/mean_depth_inv: 0.3
 * /svo/min_depth_inv: 1.0
 * /svo/n_pyr_levels: 3
 * /svo/pipeline_is_stereo: False
 * /svo/poseoptim_prior_lambda: 0.0
 * /svo/poseoptim_thresh: 2.0
 * /svo/poseoptim_using_unit_sphere: False
 * /svo/publish_every_nth_dense_input: 5
 * /svo/publish_marker_scale: 0.5
 * /svo/quality_max_drop_fts: 80
 * /svo/quality_min_fts: 40
 * /svo/reprojector_affine_est_gain: True
 * /svo/reprojector_affine_est_offset: True
 * /svo/reprojector_max_n_kfs: 5
 * /svo/scan_epi_unit_sphere: False
 * /svo/seed_convergence_sigma2_thresh: 200
 * /svo/update_seeds_with_old_keyframes: True
 * /svo/use_async_reprojectors: False
 * /svo/use_imu: False
 * /svo/use_threaded_depthfilter: False

NODES
  /
    svo (svo_ros/svo_node)

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

setting /run_id to e09e93b8-a2e2-11e9-b9a7-1cbfce0abb3b
process[rosout-1]: started with pid [1050]
started core service [/rosout]
process[svo-2]: started with pid [1068]
[ INFO] [1562743093.503454066]: Found parameter: pipeline_is_stereo, value: 0
[ WARN] [1562743093.504219749]: Cannot find value for parameter: set_initial_attitude_from_gravity, assigning default: 1
[ WARN] [1562743093.505021831]: Cannot find value for parameter: automatic_reinitialization, assigning default: 0
[ INFO] [1562743093.507050296]: Found parameter: calib_file, value: /home/fanjiaojiao/svo_ws/src/rpg_svo_example/svo_ros/svo_aero/aero_OV7251_mono.yaml
loaded 1 cameras
  name = /camera/mono/image_raw
  size = [640, 480]
  Projection = Pinhole
  Focal length = (442.091, 441.935)
  Principal point = (320.84, 236.038)
  Distortion: RadTan(0.02752, -0.064054, 0.004263, -0.000398)
[ WARN] [1562743093.515467690]: Cannot find value for parameter: klt_max_level, assigning default: 4
[ WARN] [1562743093.516167388]: Cannot find value for parameter: klt_min_level, assigning default: 0
[ INFO] [1562743093.518198678]: Found parameter: reprojector_max_n_kfs, value: 5
[ INFO] [1562743093.519945392]: Found parameter: max_fts, value: 180
[ INFO] [1562743093.521564324]: Found parameter: grid_size, value: 35
[ WARN] [1562743093.522213710]: Cannot find value for parameter: reproject_unconverged_seeds, assigning default: 1
[ INFO] [1562743093.523816515]: Found parameter: reprojector_affine_est_offset, value: 1
[ INFO] [1562743093.525662438]: Found parameter: reprojector_affine_est_gain, value: 1
[ WARN] [1562743093.526442206]: Cannot find value for parameter: init_min_features, assigning default: 100
[ WARN] [1562743093.527181132]: Cannot find value for parameter: init_min_tracked, assigning default: 80
[ WARN] [1562743093.527907753]: Cannot find value for parameter: init_min_inliers, assigning default: 70
[ INFO] [1562743093.529992869]: Found parameter: init_min_disparity, value: 30
[ WARN] [1562743093.530742031]: Cannot find value for parameter: init_min_features_factor, assigning default: 2
[ WARN] [1562743093.531481266]: Cannot find value for parameter: reproj_err_thresh, assigning default: 2
[ WARN] [1562743093.532197905]: Cannot find value for parameter: init_disparity_pivot_ratio, assigning default: 0.5
[ WARN] [1562743093.532941691]: Cannot find value for parameter: init_method, assigning default: FivePoint
[ INFO] [1562743093.535033806]: Found parameter: grid_size, value: 35
[ INFO] [1562743093.536828043]: Found parameter: n_pyr_levels, value: 3
[ INFO] [1562743093.538602832]: Found parameter: detector_threshold_primary, value: 10
[ INFO] [1562743093.540244080]: Found parameter: detector_threshold_secondary, value: 200
[ WARN] [1562743093.540896012]: Cannot find value for parameter: use_edgelets, assigning default: 1
[ INFO] [1562743093.542846781]: Found parameter: n_pyr_levels, value: 3
[ INFO] [1562743093.548325415]: Found parameter: use_threaded_depthfilter, value: 0
[ INFO] [1562743093.550341511]: Found parameter: seed_convergence_sigma2_thresh, value: 200
[ INFO] [1562743093.552192040]: Found parameter: scan_epi_unit_sphere, value: 0
[ INFO] [1562743093.554040098]: Found parameter: depth_filter_affine_est_offset, value: 1
[ INFO] [1562743093.555774692]: Found parameter: depth_filter_affine_est_gain, value: 1
[ INFO] [1562743093.557644432]: Found parameter: max_fts, value: 180
[ WARN] [1562743093.558294108]: Cannot find value for parameter: max_seeds_ratio, assigning default: 3
[ INFO] [1562743093.560000791]: Found parameter: max_n_kfs, value: 30
[ INFO] [1562743093.561775717]: Found parameter: use_imu, value: 0
[ WARN] [1562743093.587471254]: Cannot find value for parameter: trace_dir, assigning default: /home/fanjiaojiao/svo_install_ws/install/share/svo/trace
[ INFO] [1562743093.589694715]: Found parameter: quality_min_fts, value: 40
[ INFO] [1562743093.591578072]: Found parameter: quality_max_drop_fts, value: 80
[ WARN] [1562743093.592162266]: Cannot find value for parameter: relocalization_max_trials, assigning default: 50
[ INFO] [1562743093.593818815]: Found parameter: poseoptim_prior_lambda, value: 0
[ INFO] [1562743093.595560903]: Found parameter: poseoptim_using_unit_sphere, value: 0
[ INFO] [1562743093.597324561]: Found parameter: img_align_prior_lambda_rot, value: 0
[ INFO] [1562743093.599219441]: Found parameter: img_align_prior_lambda_trans, value: 0
[ WARN] [1562743093.600195145]: Cannot find value for parameter: structure_optimization_max_pts, assigning default: 20
[ INFO] [1562743093.602961818]: Found parameter: map_scale, value: 1.5
[ INFO] [1562743093.606476555]: Found parameter: kfselect_criterion, value: FORWARD
[ WARN] [1562743093.607150126]: Cannot find value for parameter: kfselect_min_dist, assigning default: 0.12
[ INFO] [1562743093.611396817]: Found parameter: kfselect_numkfs_upper_thresh, value: 180
[ INFO] [1562743093.613967192]: Found parameter: kfselect_numkfs_lower_thresh, value: 70
[ INFO] [1562743093.615794081]: Found parameter: kfselect_min_dist_metric, value: 0.001
[ INFO] [1562743093.619167110]: Found parameter: kfselect_min_angle, value: 6
[ INFO] [1562743093.621256241]: Found parameter: kfselect_min_disparity, value: 40
[ WARN] [1562743093.621953282]: Cannot find value for parameter: kfselect_min_num_frames_between_kfs, assigning default: 2
[ INFO] [1562743093.623636549]: Found parameter: img_align_max_level, value: 4
[ INFO] [1562743093.625359556]: Found parameter: img_align_min_level, value: 2
[ WARN] [1562743093.625992318]: Cannot find value for parameter: img_align_robustification, assigning default: 0
[ WARN] [1562743093.626686576]: Cannot find value for parameter: img_align_use_distortion_jacobian, assigning default: 0
[ INFO] [1562743093.629716959]: Found parameter: img_align_est_illumination_gain, value: 1
[ INFO] [1562743093.631811069]: Found parameter: img_align_est_illumination_offset, value: 1
[ INFO] [1562743093.633968485]: Found parameter: poseoptim_thresh, value: 2
[ INFO] [1562743093.635796250]: Found parameter: update_seeds_with_old_keyframes, value: 1
[ INFO] [1562743093.637478529]: Found parameter: use_async_reprojectors, value: 0
[ INFO] [1562743093.637624426]: DepthFilter: created.
[ WARN] [1562743093.638394431]: Cannot find value for parameter: T_world_imuinit/tz, assigning default: 0
[ WARN] [1562743093.639075990]: Cannot find value for parameter: T_world_imuinit/ty, assigning default: 0
[ WARN] [1562743093.639876037]: Cannot find value for parameter: T_world_imuinit/tx, assigning default: 0
[ INFO] [1562743093.642912595]: Found parameter: T_world_imuinit/qz, value: 0
[ INFO] [1562743093.645749311]: Found parameter: T_world_imuinit/qy, value: 0
[ INFO] [1562743093.647890532]: Found parameter: T_world_imuinit/qx, value: 0
[ INFO] [1562743093.649638716]: Found parameter: T_world_imuinit/qw, value: 1
[ WARN] [1562743093.650249596]: Cannot find value for parameter: publish_img_pyr_level, assigning default: 0
[ WARN] [1562743093.650898311]: Cannot find value for parameter: publish_every_nth_img, assigning default: 1
[ INFO] [1562743093.652554037]: Found parameter: publish_every_nth_dense_input, value: 5
[ WARN] [1562743093.653964653]: Cannot find value for parameter: publish_world_in_cam_frame, assigning default: 1
[ WARN] [1562743093.654587680]: Cannot find value for parameter: publish_map_every_frame, assigning default: 0
[ WARN] [1562743093.655192140]: Cannot find value for parameter: publish_point_display_time, assigning default: 0
[ WARN] [1562743093.655788095]: Cannot find value for parameter: publish_seeds, assigning default: 1
[ WARN] [1562743093.656416315]: Cannot find value for parameter: publish_seeds_uncertainty, assigning default: 0
[ WARN] [1562743093.657055286]: Cannot find value for parameter: trace_pointcloud, assigning default: 0
[ INFO] [1562743093.658713530]: Found parameter: publish_marker_scale, value: 0.5
[ INFO] [1562743093.712918778]: Found parameter: use_imu, value: 0
[INFO] SVO: SvoNode: Started Image loop.
[ WARN] [1562743093.714036782]: Cannot find value for parameter: remote_key_topic, assigning default: svo/remote_key
[ INFO] [1562743093.718255070]: Found parameter: cam0_topic, value: /camera/mono/image_raw

Many parameters couldn't be found. And I think it's the problem of eigen's version. Could you be kind to tell me which eigen version you used to do this project, please? It seems that eigen 3.2 is too old.

When I run the simulation, the drone haven't received the local position message. In offboard_module, there have topic advertise the “ /mavros/local_position/pose ”, but where does it publish ?

When I'am try to run the simulation, Gazebo is crashed...
Please, tel how to solve this problem?

GAZEBO_PLUGIN_PATH /home/gleb/catkin_ws/px4/build/posix_sitl_default/build_gazebo:
GAZEBO_MODEL_PATH :/home/gleb/catkin_ws/px4/Tools/sitl_gazebo/models
LD_LIBRARY_PATH /home/gleb/catkin_ws/devel/lib:/opt/ros/kinetic/lib:/opt/ros/kinetic/lib/x86_64-linux-gnu:/home/gleb/catkin_ws/px4/Tools/sitl_gazebo/Build/msgs/:/home/gleb/catkin_ws/px4/build/posix_sitl_default/build_gazebo
... logging to /home/gleb/.ros/log/61f06c14-e814-11e8-8ede-080027bc695c/roslaunch-gleb-VirtualBox-11350.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://gleb-VirtualBox:46511/

SUMMARY

CLEAR PARAMETERS

• /mavros/

PARAMETERS

• /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.000349065850399
• /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/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: False
• /mavros/local_position/tf/send_fcu: False
• /mavros/mission/pull_after_gcs: True
• /mavros/mocap/use_pose: True
• /mavros/mocap/use_tf: False
• /mavros/odometry/frame_tf/body_frame_orientation: frd
• /mavros/odometry/frame_tf/local_frame: vision_ned
• /mavros/plugin_blacklist: ['safety_area', '...
• /mavros/plugin_whitelist: []
• /mavros/px4flow/frame_id: px4flow
• /mavros/px4flow/ranger_fov: 0.118682389136
• /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_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: map
• /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
• /rosdistro: kinetic
• /rosversion: 1.12.14
• /use_sim_time: True
• /whycon/inner_diameter: 0.08
• /whycon/outer_diameter: 0.1952
• /whycon/targets: 1

NODES
/
gazebo (gazebo_ros/gzserver)
gazebo_gui (gazebo_ros/gzclient)
image_view (image_view/image_view)
mavros (mavros/mavros_node)
offboard_control (offboard_control/offboard_control)
rviz (rviz/rviz)
sitl (px4/px4)
trajectory_planner (trajectory_planner/trajectory_planner)
vehicle_spawn_gleb_VirtualBox_11350_5973867746794150957 (gazebo_ros/spawn_model)
whycon (whycon/whycon)

ROS_MASTER_URI=http://localhost:11311

process[sitl-1]: started with pid [11368]
node name: sitl
data path: /home/gleb/catkin_ws/px4
commands file: /home/gleb/catkin_ws/px4/posix-configs/SITL/init/ekf2/iris
138 WARNING: setRealtimeSched failed (not run as root?)

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

px4 starting.

process[gazebo-2]: started with pid [11376]
process[gazebo_gui-3]: started with pid [11381]
INFO [dataman] Unknown restart, data manager file 'rootfs/fs/microsd/dataman' size is 11405132 bytes
INFO [simulator] Waiting for initial data on UDP port 14560. Please start the flight simulator to proceed..
process[vehicle_spawn_gleb_VirtualBox_11350_5973867746794150957-4]: started with pid [11390]
process[mavros-5]: started with pid [11391]
process[image_view-6]: started with pid [11392]
process[whycon-7]: started with pid [11393]
process[trajectory_planner-8]: started with pid [11399]
process[offboard_control-9]: started with pid [11406]
process[rviz-10]: started with pid [11463]
[ INFO] [1542203505.188823035]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1542203505.194197685]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1542203505.194317657]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1542203505.194424784]: GCS bridge disabled
[ INFO] [1542203505.259229984]: Loading file whycon_transforms.yml
[ WARN] [1542203505.259442494]: Could not load "whycon_transforms.yml"
[ INFO] [1542203505.266102412]: Plugin 3dr_radio loaded
[ INFO] [1542203505.270846652]: Plugin 3dr_radio initialized
[ INFO] [1542203505.270989124]: Plugin actuator_control loaded
[ INFO] [1542203505.284609493]: Plugin actuator_control initialized
[ INFO] [1542203505.290883473]: Plugin adsb loaded
[ INFO] [1542203505.303820221]: Plugin adsb initialized
[ INFO] [1542203505.304078769]: Plugin altitude loaded
[ INFO] [1542203505.306792440]: Plugin altitude initialized
[ INFO] [1542203505.306996869]: Plugin cam_imu_sync loaded
[ INFO] [1542203505.308453868]: Plugin cam_imu_sync initialized
[ INFO] [1542203505.308670784]: Plugin command loaded
[ INFO] [1542203505.349226044]: Plugin command initialized
[ INFO] [1542203505.349623746]: Plugin debug_value loaded
[ INFO] [1542203505.375452064]: Plugin debug_value initialized
[ INFO] [1542203505.375504741]: Plugin distance_sensor blacklisted
[ INFO] [1542203505.375702024]: Plugin fake_gps loaded
[ INFO] [1542203505.455923403]: Plugin fake_gps initialized
[ INFO] [1542203505.456173764]: Plugin ftp loaded
[ INFO] [1542203505.485806562]: Plugin ftp initialized
[ INFO] [1542203505.486059645]: Plugin global_position loaded
[ INFO] [1542203505.663500394]: Plugin global_position initialized
[ INFO] [1542203505.663723036]: Plugin gps_rtk loaded
[ INFO] [1542203505.702213763]: Plugin gps_rtk initialized
[ INFO] [1542203505.702516714]: Plugin hil loaded
[ INFO] [1542203505.782027038]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1542203505.784181484]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1542203505.859054162]: Plugin hil initialized
[ INFO] [1542203505.859305781]: Plugin home_position loaded
[ INFO] [1542203505.868240168]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1542203505.869345971]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1542203505.876925366]: Plugin home_position initialized
[ INFO] [1542203505.877182850]: Plugin imu loaded
[ INFO] [1542203505.965255745]: Plugin imu initialized
[ INFO] [1542203505.965497302]: Plugin local_position loaded
[ INFO] [1542203505.982747899]: Plugin local_position initialized
[ INFO] [1542203505.982960132]: Plugin log_transfer loaded
[ INFO] [1542203506.034194753]: Plugin log_transfer initialized
[ INFO] [1542203506.034523213]: Plugin manual_control loaded
[ INFO] [1542203506.064776477]: Plugin manual_control initialized
[ INFO] [1542203506.064975883]: Plugin mocap_pose_estimate loaded
[ INFO] [1542203506.081266985]: Plugin mocap_pose_estimate initialized
[ INFO] [1542203506.081530874]: Plugin obstacle_distance loaded
[ INFO] [1542203506.087675265]: Plugin obstacle_distance initialized
[ INFO] [1542203506.087998036]: Plugin odom loaded
[ INFO] [1542203506.096738932]: Plugin odom initialized
[ INFO] [1542203506.097045612]: Plugin param loaded
[ INFO] [1542203506.116808843]: Plugin param initialized
[ INFO] [1542203506.117069188]: Plugin px4flow loaded
[ INFO] [1542203506.145486656]: Using transport "raw"
[ INFO] [1542203506.170946140]: Plugin px4flow initialized
[ INFO] [1542203506.171006461]: Plugin rangefinder blacklisted
[ INFO] [1542203506.171277278]: Plugin rc_io loaded
[ INFO] [1542203506.185396347]: Plugin rc_io initialized
[ INFO] [1542203506.185459061]: Plugin safety_area blacklisted
[ INFO] [1542203506.185772152]: Plugin setpoint_accel loaded
[ INFO] [1542203506.199605285]: Plugin setpoint_accel initialized
[ INFO] [1542203506.199848090]: Plugin setpoint_attitude loaded
[ INFO] [1542203506.290251820]: Plugin setpoint_attitude initialized
[ INFO] [1542203506.290543699]: Plugin setpoint_position loaded
[ INFO] [1542203506.405884669]: Plugin setpoint_position initialized
[ INFO] [1542203506.406145806]: Plugin setpoint_raw loaded
[ INFO] [1542203506.485738259]: Plugin setpoint_raw initialized
[ INFO] [1542203506.486043694]: Plugin setpoint_velocity loaded
[ INFO] [1542203506.497144625]: Plugin setpoint_velocity initialized
[ INFO] [1542203506.497425035]: Plugin sys_status loaded
[ INFO] [1542203506.521983054]: Plugin sys_status initialized
[ INFO] [1542203506.522239563]: Plugin sys_time loaded
[ INFO] [1542203506.600423179]: TM: Timesync mode: MAVLINK
[ INFO] [1542203506.604360497]: Plugin sys_time initialized
[ INFO] [1542203506.604589543]: Plugin trajectory loaded
[ INFO] [1542203506.696409022]: Plugin trajectory initialized
[ INFO] [1542203506.696658229]: Plugin vfr_hud loaded
[ INFO] [1542203506.697615103]: Plugin vfr_hud initialized
[ INFO] [1542203506.697688932]: Plugin vibration blacklisted
[ INFO] [1542203506.697867929]: Plugin vision_pose_estimate loaded
[ INFO] [1542203506.731083055]: Plugin vision_pose_estimate initialized
[ INFO] [1542203506.731397146]: Plugin vision_speed_estimate loaded
[ INFO] [1542203506.744171584]: Plugin vision_speed_estimate initialized
[ INFO] [1542203506.744435273]: Plugin waypoint loaded
[ INFO] [1542203506.765927861]: Plugin waypoint initialized
[ INFO] [1542203506.766180746]: Plugin wind_estimation loaded
[ INFO] [1542203506.767628664]: Plugin wind_estimation initialized
[ INFO] [1542203506.767695775]: Autostarting mavlink via USB on PX4
[ INFO] [1542203506.767924377]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1542203506.767974584]: Built-in MAVLink package version: 2018.10.10
[ INFO] [1542203506.768015961]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1542203506.768068895]: MAVROS started. MY ID 1.240, TARGET ID 1.1
SpawnModel script started
[INFO] [1542203507.317114, 0.000000]: Loading model XML from file
[INFO] [1542203507.317524, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[INFO] [1542203508.230323, 0.000000]: Calling service /gazebo/spawn_sdf_model
[ INFO] [1542203511.150599516, 0.002000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1542203511.186807299, 0.002000000]: Camera Plugin (ns = /) <tf_prefix_>, set to ""
[ INFO] [1542203511.195487749, 0.002000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ERROR] [1542203511.196492402, 0.002000000]: Tried to advertise a service that is already advertised in this node [/camera/set_camera_info]
[ INFO] [1542203511.202761004, 0.002000000]: Camera Plugin (ns = /) <tf_prefix_>, set to ""
[ERROR] [1542203511.202937406, 0.002000000]: Tried to advertise a service that is already advertised in this node [/camera/set_parameters]
[INFO] [1542203511.216599, 0.002000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1542203511.268092427, 0.002000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ERROR] [1542203511.315160514, 0.002000000]: Tried to advertise a service that is already advertised in this node [/camera/set_camera_info]
[ INFO] [1542203511.332080992, 0.002000000]: Camera Plugin (ns = /) <tf_prefix_>, set to ""
[ERROR] [1542203511.333970532, 0.002000000]: Tried to advertise a service that is already advertised in this node [/camera/set_parameters]
[ INFO] [1542203511.602053978, 0.036000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1542203511.602569828, 0.036000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1542203511.875940132, 0.082000000]: Physics dynamic reconfigure ready.
[vehicle_spawn_gleb_VirtualBox_11350_5973867746794150957-4] process has finished cleanly
log file: /home/gleb/.ros/log/61f06c14-e814-11e8-8ede-080027bc695c/vehicle_spawn_gleb_VirtualBox_11350_5973867746794150957-4*.log
[ INFO] [1542203511.959168905, 0.120000000]: Physics dynamic reconfigure ready.
INFO [simulator] Got initial simulation data, running sim..
INFO [pwm_out_sim] MODE_16PWM
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14556 remote port 14550
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14557 remote port 14540
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
pxh> INFO [logger] logger started (mode=all)
INFO [logger] Start file log
INFO [logger] Opened log file: rootfs/fs/microsd/log/2018-11-14/13_51_52.ulg
INFO [mavlink] partner IP: 127.0.0.1
[ INFO] [1542203512.823848303, 0.600000000]: udp0: Remote address: 127.0.0.1:14557
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
[ INFO] [1542203513.092884767, 0.722000000]: IMU: High resolution IMU detected!
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
INFO [Unknown] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
[ INFO] [1542203514.423794044, 1.438000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1542203514.430152136, 1.444000000]: IMU: High resolution IMU detected!
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
[ INFO] [1542203514.614491879, 1.522000000]: IMU: Attitude quaternion IMU detected!
[ INFO] [1542203514.712142762, 1.556000000]: Drone to camera transform initialized
ERROR [mavlink] [timesync] Hard setting offset.
ERROR [mavlink] [timesync] Hard setting offset.
gzserver: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreRenderSystem.cpp:546: virtual void Ogre::RenderSystem::setDepthBufferFor(Ogre::RenderTarget*): Assertion bAttached && "A new DepthBuffer for a RenderTarget was created, but after creation" "it says it's incompatible with that RT"' failed. Aborted (core dumped) [gazebo-2] process has died [pid 11376, exit code 134, cmd /opt/ros/kinetic/lib/gazebo_ros/gzserver -e ode /home/gleb/catkin_ws/px4/Tools/sitl_gazebo/worlds/empty.world __name:=gazebo __log:=/home/gleb/.ros/log/61f06c14-e814-11e8-8ede-080027bc695c/gazebo-2.log]. log file: /home/gleb/.ros/log/61f06c14-e814-11e8-8ede-080027bc695c/gazebo-2*.log gzclient: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreRenderSystem.cpp:546: virtual void Ogre::RenderSystem::setDepthBufferFor(Ogre::RenderTarget*): Assertion bAttached && "A new DepthBuffer for a RenderTarget was created, but after creation" "it says it's incompatible with that RT"' failed.
Aborted (core dumped)
[gazebo_gui-3] process has died [pid 11381, exit code 134, cmd /opt/ros/kinetic/lib/gazebo_ros/gzclient __name:=gazebo_gui __log:=/home/gleb/.ros/log/61f06c14-e814-11e8-8ede-080027bc695c/gazebo_gui-3.log].
log file: /home/gleb/.ros/log/61f06c14-e814-11e8-8ede-080027bc695c/gazebo_gui-3*.log

Hi @szebedy
Thanks for your sharing.
I have some question about the simulation.

1. Your introduction(simulation launch everything in the same window with a launch file) said we need launch svo_ros intel_aero. launch like this:
   
   I'm wondering could I use other algorithm's launch file(maybe I can write a new launch.file for my algorithm like yours) instead of it?
2. For launch each component separately method.
   I tried to do the test step by step as your introduction but when I run the "source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/posix_sitl_default" it will show this error:
   
   could you give me some advice to solve this problem?

Thanks for your sharing again It's really helpful!

Hi,
I'm trying to launch the project in simulation mode.
I'm using
roslaunch svo_ros intel_aero.launch and
roslaunch offboard_control simulation.launch.

But only empty world started in Gazebo,
and the drone starts to perform marker searching pattern.
There is no environment and no camera input observed.
Is there needed some additional packages to simulate
the world and camera inputs as shown in example video.

Hi szebedy,
Thank you for sharing your code, very interesting.
In order to use visual odometry (SVO), we have to update the PX4 version on the intel aero ready to fly drone.
Unfortunately, I was unable to compile the px4 version v1.8.2 on intel RTF done (by following that two links (1) https://docs.px4.io/en/complete_vehicles/intel_aero.html and (2) https://dev.px4.io/en/setup/building_px4.html)

Can you tell me how did you manage to do that?

Thank you very much for your help

i tried using the second approach to launch the simulation and the following are the screenshots from the 7 terminals
the problem i face is that the drone doesn't take off at all
i followed all the installation steps properly

output of mavros_posix_sitl.launch

output of rosrun whycon whycon camera/image_rect_color:=/camera/rgb/image_raw camera/camera_info:=/camera/rgb/camera_info _targets:=1 _inner_diameter:=0.08 _outer_diameter:=0.1952

output of roslaunch svo_ros intel_aero.launch

output of gazebo window