roslaunch local_planner local_planner_stereo.launch logfile

gistfile1.log

arek@L2D2:~$ roslaunch local_planner local_planner_stereo.launch world_path:=/home/arek/catkin_ws/src/avoidance/sim/worlds/test_city_2.world
... logging to /home/arek/.ros/log/225781da-6d5a-11e8-8d26-f49634e9c82a/roslaunch-L2D2-32155.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://L2D2:40325/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /gazebo/enable_ros_network: True
 * /local_planner_node/depth_points_topic: /stereo/points2
 * /local_planner_node/goal_x_param: 15
 * /local_planner_node/goal_y_param: 15
 * /local_planner_node/goal_z_param: 4
 * /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: ROLL_180
 * /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: ROLL_180
 * /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: ROLL_180
 * /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: ROLL_180
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: udp://:14540@loca...
 * /mavros/gcs_url: 
 * /mavros/global_position/frame_id: fcu
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: fcu_utm
 * /mavros/global_position/tf/frame_id: local_origin
 * /mavros/global_position/tf/send: False
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.000349065850399
 * /mavros/imu/frame_id: fcu
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/local_position/frame_id: local_origin
 * /mavros/local_position/tf/child_frame_id: fcu
 * /mavros/local_position/tf/frame_id: local_origin
 * /mavros/local_position/tf/send: True
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mocap/use_pose: False
 * /mavros/mocap/use_tf: True
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.0
 * /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_throttle: False
 * /mavros/setpoint_attitude/tf/child_frame_id: attitude
 * /mavros/setpoint_attitude/tf/frame_id: local_origin
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 10.0
 * /mavros/setpoint_position/tf/child_frame_id: setpoint
 * /mavros/setpoint_position/tf/frame_id: local_origin
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /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/vibration/frame_id: vibration
 * /mavros/vision_pose/tf/child_frame_id: vision
 * /mavros/vision_pose/tf/frame_id: local_origin
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: melodic
 * /rosversion: 1.14.1
 * /stereo/stereo_image_proc/correlation_window_size: 19.0
 * /stereo/stereo_image_proc/disparity_range: 32.0
 * /stereo/stereo_image_proc/speckle_range: 2.0
 * /stereo/stereo_image_proc/speckle_size: 1000.0
 * /stereo/stereo_image_proc/stereo_algorithm: 1.0
 * /stereo/stereo_image_proc/uniqueness_ratio: 40.0
 * /use_sim_time: True

NODES
  /stereo/
    stereo_image_proc (stereo_image_proc/stereo_image_proc)
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    local_planner_node (local_planner/local_planner_node)
    mavros (mavros/mavros_node)
    rqt_reconfigure (rqt_reconfigure/rqt_reconfigure)
    rviz (rviz/rviz)
    sitl (px4/px4)
    spawn_model (gazebo_ros/spawn_model)
    tf_90_deg (tf/static_transform_publisher)
    tf_depth_camera (tf/static_transform_publisher)

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

setting /run_id to 225781da-6d5a-11e8-8d26-f49634e9c82a
process[rosout-1]: started with pid [32176]
started core service [/rosout]
process[rqt_reconfigure-2]: started with pid [32184]
process[tf_90_deg-3]: started with pid [32185]
process[tf_depth_camera-4]: started with pid [32186]
process[sitl-5]: started with pid [32199]
node name: sitl
data path: /home/arek/Firmware
commands file: /home/arek/Firmware/posix-configs/SITL/init/ekf2/iris
76 WARNING: setRealtimeSched failed (not run as root?)

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

px4 starting.

process[mavros-6]: started with pid [32207]
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[gazebo-7]: started with pid [32210]
process[gazebo_gui-8]: started with pid [32217]
process[spawn_model-9]: started with pid [32223]
process[stereo/stereo_image_proc-10]: started with pid [32228]
process[local_planner_node-11]: started with pid [32229]
process[rviz-12]: started with pid [32232]
[ INFO] [1528709453.354846944]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1528709453.358182849]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1528709453.358344211]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1528709453.358683494]: GCS bridge disabled
[ INFO] [1528709453.373950354]: Plugin 3dr_radio loaded
[ INFO] [1528709453.377380260]: Plugin 3dr_radio initialized
[ INFO] [1528709453.377600980]: Plugin actuator_control loaded
[ INFO] [1528709453.384550512]: Plugin actuator_control initialized
[ INFO] [1528709453.388564734]: Plugin adsb loaded
[ INFO] [1528709453.394608805]: Plugin adsb initialized
[ INFO] [1528709453.396103586]: Plugin altitude loaded
[ INFO] [1528709453.398285025]: Plugin altitude initialized
[ INFO] [1528709453.398519948]: Plugin cam_imu_sync loaded
[ INFO] [1528709453.399941113]: Plugin cam_imu_sync initialized
[ INFO] [1528709453.400216885]: Plugin command loaded
[ INFO] [1528709453.408336950]: Plugin command initialized
[ INFO] [1528709453.408523178]: Plugin debug_value loaded
[ INFO] [1528709453.415097817]: Plugin debug_value initialized
[ INFO] [1528709453.415164476]: Plugin distance_sensor blacklisted
[ INFO] [1528709453.415406427]: Plugin fake_gps loaded
[ INFO] [1528709453.434633082]: Plugin fake_gps initialized
[ INFO] [1528709453.434858833]: Plugin ftp loaded
[ INFO] [1528709453.458324729]: Plugin ftp initialized
[ INFO] [1528709453.458515481]: Plugin global_position loaded
[DEBUG] [1528709453.481863675]: Dynamic reconfigure call
[ INFO] [1528709453.488549045]: Plugin global_position initialized
[ INFO] [1528709453.488731817]: Plugin hil loaded
[ INFO] [1528709453.511166962]: Plugin hil initialized
[ INFO] [1528709453.511358701]: Plugin home_position loaded
[ INFO] [1528709453.517294499]: Plugin home_position initialized
[ INFO] [1528709453.517441101]: Plugin imu loaded
[ INFO] [1528709453.526031431]: rviz version 1.13.1
[ INFO] [1528709453.526109762]: compiled against Qt version 5.9.5
[ INFO] [1528709453.526133217]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1528709453.537263704]: Plugin imu initialized
[ INFO] [1528709453.537453209]: Plugin local_position loaded
[ INFO] [1528709453.552814642]: Plugin local_position initialized
[ INFO] [1528709453.553006673]: Plugin manual_control loaded
[ INFO] [1528709453.557345621]: Plugin manual_control initialized
[ INFO] [1528709453.557519316]: Plugin mocap_pose_estimate loaded
[ INFO] [1528709453.566148406]: Plugin mocap_pose_estimate initialized
[ INFO] [1528709453.566310210]: Plugin obstacle_distance loaded
[ INFO] [1528709453.571329842]: Plugin obstacle_distance initialized
[ INFO] [1528709453.571494619]: Plugin odom loaded
[FATAL] [1528709453.577279224]: ODOM: invalid body frame orientation "frd"
[ INFO] [1528709453.577385576]: Plugin odom initialized
[ INFO] [1528709453.577606440]: Plugin param loaded
[ INFO] [1528709453.583441832]: Plugin param initialized
[ INFO] [1528709453.583695386]: Plugin px4flow loaded
[ INFO] [1528709453.595838963]: Plugin px4flow initialized
[ INFO] [1528709453.595886277]: Plugin rangefinder blacklisted
[ INFO] [1528709453.596207947]: Plugin rc_io loaded
[ INFO] [1528709453.602330628]: Plugin rc_io initialized
[ INFO] [1528709453.602985408]: Plugin rtcm_inject loaded
[ INFO] [1528709453.607519699]: Plugin rtcm_inject initialized
[ INFO] [1528709453.607573947]: Plugin safety_area blacklisted
libpng warning: iCCP: known incorrect sRGB profile
[ INFO] [1528709453.607750196]: Plugin setpoint_accel loaded
libpng warning: iCCP: known incorrect sRGB profile
[ INFO] [1528709453.614259972]: Plugin setpoint_accel initialized
[ INFO] [1528709453.614538302]: Plugin setpoint_attitude loaded
[ INFO] [1528709453.620389539]: ===== Set Goal ======: [15.000000, 15.000000, 4.000000].
[ INFO] [1528709453.630344991]: Plugin setpoint_attitude initialized
[ INFO] [1528709453.630612323]: Plugin setpoint_position loaded
[ INFO] [1528709453.668583659]: Plugin setpoint_position initialized
[ INFO] [1528709453.668798457]: Plugin setpoint_raw loaded
[ INFO] [1528709453.696651149]: Plugin setpoint_raw initialized
[ INFO] [1528709453.696895164]: Plugin setpoint_velocity loaded
[ INFO] [1528709453.703317989]: Plugin setpoint_velocity initialized
[ INFO] [1528709453.703662974]: Plugin sys_status loaded
[ INFO] [1528709453.720263776]: Plugin sys_status initialized
[ INFO] [1528709453.720490854]: Plugin sys_time loaded
[ INFO] [1528709453.725282158]: TM: Timesync mode: MAVLINK
[ INFO] [1528709453.726609285]: Plugin sys_time initialized
[ INFO] [1528709453.726801431]: Plugin trajectory loaded
[ INFO] [1528709453.735355382]: Plugin trajectory initialized
[ INFO] [1528709453.735563066]: Plugin vfr_hud loaded
[ INFO] [1528709453.737729620]: Plugin vfr_hud initialized
[ INFO] [1528709453.737786232]: Plugin vibration blacklisted
[ INFO] [1528709453.738046401]: Plugin vision_pose_estimate loaded
[ INFO] [1528709453.759576793]: Plugin vision_pose_estimate initialized
[ INFO] [1528709453.759969262]: Plugin vision_speed_estimate loaded
[ INFO] [1528709453.767614065]: Plugin vision_speed_estimate initialized
[ INFO] [1528709453.767960928]: Plugin waypoint loaded
[ INFO] [1528709453.778986411]: Plugin waypoint initialized
[ INFO] [1528709453.779212613]: Plugin wind_estimation loaded
[ INFO] [1528709453.780809958]: Plugin wind_estimation initialized
[ INFO] [1528709453.780882306]: Autostarting mavlink via USB on PX4
[ INFO] [1528709453.781050012]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1528709453.781095279]: Built-in MAVLink package version: 2018.5.5
[ INFO] [1528709453.781132378]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1528709453.781297404]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1528709453.848875301]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1528709453.851300363]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1528709453.924123920]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1528709453.925200583]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
SpawnModel script started
[INFO] [1528709454.336325, 0.000000]: Loading model XML from Gazebo Model Database
[INFO] [1528709454.337074, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1528709455.389745873]: Stereo is NOT SUPPORTED
[ INFO] [1528709455.390491704]: OpenGl version: 4.6 (GLSL 4.6).
libpng warning: iCCP: known incorrect sRGB profile
[INFO] [1528709456.445312, 786.970000]: Calling service /gazebo/spawn_sdf_model
[ INFO] [1528709456.456844378, 786.982000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1528709456.469632660, 786.994000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [1528709456.761029, 787.160000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1528709456.774264128, 787.160000000]: Physics dynamic reconfigure ready.
[ INFO] [1528709456.774386928, 787.160000000]: Physics dynamic reconfigure ready.
[ INFO] [1528709456.975942623, 787.160000000]: Camera Plugin: The 'robotNamespace' param did not exit
[ INFO] [1528709456.976103945, 787.160000000]: Camera Plugin: The 'robotNamespace' param did not exit
[ INFO] [1528709456.978541355, 787.160000000]: Camera Plugin (ns = )  <tf_prefix_>, set to ""
[ INFO] [1528709456.978588896, 787.160000000]: Camera Plugin (ns = )  <tf_prefix_>, set to ""
[spawn_model-9] process has finished cleanly
log file: /home/arek/.ros/log/225781da-6d5a-11e8-8d26-f49634e9c82a/spawn_model-9*.log
INFO  [simulator] Got initial simulation data, running sim..
INFO  [pwm_out_sim] MODE_16PWM
ERROR [sensors] no ADC found: /dev/adc0 (9)
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
WARN  [mavlink] stream SCALED_IMU2 not found
WARN  [mavlink] stream SCALED_IMU3 not found
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO  [logger] logger started (mode=all)
pxh> INFO  [logger] Start file log
INFO  [logger] Opened log file: rootfs/fs/microsd/log/2018-06-11/09_30_58.ulg
INFO  [mavlink] partner IP: 127.0.0.1
[ INFO] [1528709458.278410265, 788.324000000]: udp0: Remote address: 127.0.0.1:14557
[ WARN] [1528709458.278633459, 788.324000000]: TM: Clock skew detected (-788.134471000 s). Hard syncing clocks.
[ INFO] [1528709458.419545229, 788.464000000]: IMU: High resolution IMU detected!
[ WARN] [1528709458.928128475, 788.972000000]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1528709459.027777447, 789.072000000]: MSG to TF: Quaternion Not Properly Normalized
INFO  [Unknown] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
[ WARN] [1528709459.127413534, 789.172000000]: MSG to TF: Quaternion Not Properly Normalized
[ INFO] [1528709459.186261137, 789.230000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1528709459.200404791, 789.244000000]: IMU: High resolution IMU detected!
[ WARN] [1528709459.226980560, 789.272000000]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1528709459.227050149, 789.272000000]:  Pointcloud timeout: No position received, no WP to output.... 
 
[ INFO] [1528709459.323369571, 789.368000000]: IMU: Attitude quaternion IMU detected!
[ INFO] [1528709459.328179075, 789.372000000]:  Pointcloud timeout: Hovering at current position 
 
[ INFO] [1528709459.427920982, 789.472000000]:  Pointcloud timeout: Hovering at current position 
 
[ INFO] [1528709459.528309431, 789.572000000]:  Pointcloud timeout: Hovering at current position 
 
[ INFO] [1528709459.627942847, 789.672000000]:  Pointcloud timeout: Hovering at current position 
 
[ INFO] [1528709459.727509155, 789.772000000]:  Pointcloud timeout: Hovering at current position 
 
[ INFO] [1528709459.828152529, 789.872000000]:  Pointcloud timeout: Hovering at current position 
 
[ INFO] [1528709459.928255719, 789.972000000]:  Pointcloud timeout: Hovering at current position 
 
[ INFO] [1528709460.028365170, 790.072000000]:  Pointcloud timeout: Hovering at current position