lkumarbe@intel-H97M-D3H:~/src/gh-PX4/Firmware$ roslaunch avoidance global_planner_sitl_mavros.launch ... logging to /home/lkumarbe/.ros/log/1a38f564-bd52-11e7-9ac9-1c1b0d1ecbab/roslaunch-intel-H97M-D3H-5886.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://intel-H97M-D3H:40644/ SUMMARY ======== CLEAR PARAMETERS * /mavros/ PARAMETERS * /global_planner_node/start_pos_x: 0.5 * /global_planner_node/start_pos_y: 0.5 * /global_planner_node/start_pos_z: 3.5 * /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_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 * /octomap_server/color/a: 1.0 * /octomap_server/color/b: 0.1 * /octomap_server/color/g: 0.1 * /octomap_server/color/r: 0.1 * /octomap_server/frame_id: world * /octomap_server/height_map: False * /octomap_server/occupancy_min_z: 1.0 * /octomap_server/publish_free_space: False * /octomap_server/resolution: 1.0 * /octomap_server/sensor_model/hit: 0.9 * /octomap_server/sensor_model/max: 0.99 * /octomap_server/sensor_model/max_range: 9.0 * /octomap_server/sensor_model/min: 0.01 * /octomap_server/sensor_model/miss: 0.45 * /path_handler_node/start_pos_x: 0.5 * /path_handler_node/start_pos_y: 0.5 * /path_handler_node/start_pos_z: 3.5 * /path_handler_node/three_point_mode_: False * /rosdistro: kinetic * /rosversion: 1.12.7 * /use_sim_time: True NODES / gazebo (gazebo_ros/gzserver) gazebo_gui (gazebo_ros/gzclient) global_planner_node (avoidance/global_planner_node) mavros (mavros/mavros_node) octomap_server (octomap_server/octomap_server_node) path_handler_node (avoidance/path_handler_node) rqt_reconfigure (rqt_reconfigure/rqt_reconfigure) rviz (rviz/rviz) sitl (px4/px4) spawn_model (gazebo_ros/spawn_model) tf_camera (tf/static_transform_publisher) tf_local_origin (tf/static_transform_publisher) auto-starting new master process[master]: started with pid [5897] ROS_MASTER_URI=http://localhost:11311 setting /run_id to 1a38f564-bd52-11e7-9ac9-1c1b0d1ecbab process[rosout-1]: started with pid [5910] started core service [/rosout] process[rqt_reconfigure-2]: started with pid [5935] process[tf_local_origin-3]: started with pid [5936] process[tf_camera-4]: started with pid [5937] process[sitl-5]: started with pid [5985] node name: sitl data path: /home/lkumarbe/src/gh-PX4/Firmware commands file: /home/lkumarbe/src/gh-PX4/Firmware/posix-configs/SITL/init/ekf2/iris 48 WARNING: setRealtimeSched failed (not run as root?) ______ __ __ ___ | ___ \ \ \ / / / | | |_/ / \ V / / /| | | __/ / \ / /_| | | | / /^\ \ \___ | \_| \/ \/ |_/ px4 starting. process[mavros-6]: started with pid [5993] process[gazebo-7]: started with pid [5996] process[gazebo_gui-8]: started with pid [6001] process[spawn_model-9]: started with pid [6006] process[global_planner_node-10]: started with pid [6007] process[path_handler_node-11]: started with pid [6008] process[octomap_server-12]: started with pid [6009] INFO [dataman] Unknown restart, data manager file 'rootfs/fs/microsd/dataman' size is 11405140 bytes INFO [platforms__posix__drivers__ledsim] LED::init INFO [platforms__posix__drivers__ledsim] LED::init INFO [simulator] Waiting for initial data on UDP port 14560. Please start the flight simulator to proceed.. process[rviz-13]: started with pid [6324] [ INFO] [1509354600.896879609]: rviz version 1.12.11 [ INFO] [1509354600.896929113]: compiled against Qt version 5.5.1 [ INFO] [1509354600.896938968]: compiled against OGRE version 1.9.0 (Ghadamon) [ INFO] [1509354600.932831281]: FCU URL: udp://:14540@localhost:14557 [ WARN] [1509354600.933627844]: init: message from ASLUAV, MSG-ID 201 ignored! Table has different entry. [ WARN] [1509354600.933801666]: init: message from autoquad, MSG-ID 150 ignored! Table has different entry. [ WARN] [1509354600.933816432]: init: message from autoquad, MSG-ID 152 ignored! Table has different entry. [ WARN] [1509354600.933950415]: init: message from matrixpilot, MSG-ID 150 ignored! Table has different entry. [ WARN] [1509354600.933972019]: init: message from matrixpilot, MSG-ID 151 ignored! Table has different entry. [ WARN] [1509354600.933980246]: init: message from matrixpilot, MSG-ID 152 ignored! Table has different entry. [ WARN] [1509354600.933989067]: init: message from matrixpilot, MSG-ID 153 ignored! Table has different entry. [ WARN] [1509354600.933997454]: init: message from matrixpilot, MSG-ID 155 ignored! Table has different entry. [ WARN] [1509354600.934006436]: init: message from matrixpilot, MSG-ID 156 ignored! Table has different entry. [ WARN] [1509354600.934014277]: init: message from matrixpilot, MSG-ID 157 ignored! Table has different entry. [ WARN] [1509354600.934022376]: init: message from matrixpilot, MSG-ID 158 ignored! Table has different entry. [ WARN] [1509354600.934030974]: init: message from matrixpilot, MSG-ID 170 ignored! Table has different entry. [ WARN] [1509354600.934038821]: init: message from matrixpilot, MSG-ID 171 ignored! Table has different entry. [ WARN] [1509354600.934046940]: init: message from matrixpilot, MSG-ID 172 ignored! Table has different entry. [ WARN] [1509354600.934054730]: init: message from matrixpilot, MSG-ID 173 ignored! Table has different entry. [ WARN] [1509354600.934063028]: init: message from matrixpilot, MSG-ID 174 ignored! Table has different entry. [ WARN] [1509354600.934070822]: init: message from matrixpilot, MSG-ID 175 ignored! Table has different entry. [ WARN] [1509354600.934078376]: init: message from matrixpilot, MSG-ID 176 ignored! Table has different entry. [ WARN] [1509354600.934086018]: init: message from matrixpilot, MSG-ID 177 ignored! Table has different entry. [ WARN] [1509354600.934094048]: init: message from matrixpilot, MSG-ID 178 ignored! Table has different entry. [ WARN] [1509354600.934101756]: init: message from matrixpilot, MSG-ID 179 ignored! Table has different entry. [ WARN] [1509354600.934109815]: init: message from matrixpilot, MSG-ID 180 ignored! Table has different entry. [ WARN] [1509354600.934117685]: init: message from matrixpilot, MSG-ID 181 ignored! Table has different entry. [ WARN] [1509354600.934125667]: init: message from matrixpilot, MSG-ID 182 ignored! Table has different entry. [ WARN] [1509354600.934133325]: init: message from matrixpilot, MSG-ID 183 ignored! Table has different entry. [ WARN] [1509354600.934141038]: init: message from matrixpilot, MSG-ID 184 ignored! Table has different entry. [ WARN] [1509354600.934149215]: init: message from matrixpilot, MSG-ID 185 ignored! Table has different entry. [ WARN] [1509354600.934157124]: init: message from matrixpilot, MSG-ID 186 ignored! Table has different entry. [ WARN] [1509354600.934278407]: init: message from paparazzi, MSG-ID 180 ignored! Table has different entry. [ WARN] [1509354600.934286374]: init: message from paparazzi, MSG-ID 181 ignored! Table has different entry. [ WARN] [1509354600.934293983]: init: message from paparazzi, MSG-ID 182 ignored! Table has different entry. [ WARN] [1509354600.934301577]: init: message from paparazzi, MSG-ID 183 ignored! Table has different entry. [ WARN] [1509354600.934309279]: init: message from paparazzi, MSG-ID 184 ignored! Table has different entry. [ WARN] [1509354600.934424082]: init: message from slugs, MSG-ID 170 ignored! Table has different entry. [ WARN] [1509354600.934431658]: init: message from slugs, MSG-ID 172 ignored! Table has different entry. [ WARN] [1509354600.934439850]: init: message from slugs, MSG-ID 173 ignored! Table has different entry. [ WARN] [1509354600.934447574]: init: message from slugs, MSG-ID 176 ignored! Table has different entry. [ WARN] [1509354600.934455309]: init: message from slugs, MSG-ID 177 ignored! Table has different entry. [ WARN] [1509354600.934462905]: init: message from slugs, MSG-ID 179 ignored! Table has different entry. [ WARN] [1509354600.934470764]: init: message from slugs, MSG-ID 180 ignored! Table has different entry. [ WARN] [1509354600.934478366]: init: message from slugs, MSG-ID 181 ignored! Table has different entry. [ WARN] [1509354600.934485827]: init: message from slugs, MSG-ID 184 ignored! Table has different entry. [ WARN] [1509354600.934493459]: init: message from slugs, MSG-ID 185 ignored! Table has different entry. [ WARN] [1509354600.934501572]: init: message from slugs, MSG-ID 186 ignored! Table has different entry. [ WARN] [1509354600.934509032]: init: message from slugs, MSG-ID 188 ignored! Table has different entry. [ WARN] [1509354600.934518017]: init: message from slugs, MSG-ID 191 ignored! Table has different entry. [ WARN] [1509354600.934526108]: init: message from slugs, MSG-ID 192 ignored! Table has different entry. [ WARN] [1509354600.934533984]: init: message from slugs, MSG-ID 193 ignored! Table has different entry. [ WARN] [1509354600.934541832]: init: message from slugs, MSG-ID 194 ignored! Table has different entry. [ INFO] [1509354600.935085649]: udp0: Bind address: 0.0.0.0:14540 [ INFO] [1509354600.935141074]: udp0: Remote address: 127.0.0.1:14557 [ INFO] [1509354600.935323417]: GCS bridge disabled [ INFO] [1509354600.996321888]: Stereo is NOT SUPPORTED [ INFO] [1509354600.996391161]: OpenGl version: 3 (GLSL 1.3). SpawnModel script started [ INFO] [1509354601.134647954]: Plugin 3dr_radio loaded [ INFO] [1509354601.137054501]: Plugin 3dr_radio initialized [ INFO] [1509354601.137177947]: Plugin actuator_control loaded [ INFO] [1509354601.141688172]: Plugin actuator_control initialized [ INFO] [1509354601.198896008]: Plugin adsb loaded [ INFO] [1509354601.202680445]: Plugin adsb initialized [ INFO] [1509354601.202860406]: Plugin altitude loaded [ INFO] [1509354601.204014783]: Plugin altitude initialized [ INFO] [1509354601.204178853]: Plugin cam_imu_sync loaded [ INFO] [1509354601.204838186]: Plugin cam_imu_sync initialized [ INFO] [1509354601.204991737]: Plugin command loaded [ INFO] [1509354601.210504635]: Plugin command initialized [ INFO] [1509354601.210547043]: Plugin distance_sensor blacklisted [ INFO] [1509354601.210713398]: Plugin fake_gps loaded [ INFO] [1509354601.223310384]: Plugin fake_gps initialized [ INFO] [1509354601.223559465]: Plugin ftp loaded [INFO] [1509354601.224193, 0.000000]: Loading model XML from Gazebo Model Database [INFO] [1509354601.224415, 0.000000]: Waiting for service /gazebo/spawn_sdf_model [ INFO] [1509354601.230270135]: Plugin ftp initialized [ INFO] [1509354601.230434737]: Plugin global_position loaded [ INFO] [1509354601.241997731]: Plugin global_position initialized [ INFO] [1509354601.242165511]: Plugin hil loaded [ INFO] [1509354601.252744199]: Plugin hil initialized [ INFO] [1509354601.253015889]: Plugin home_position loaded [ INFO] [1509354601.256348596]: Plugin home_position initialized [ INFO] [1509354601.256533100]: Plugin imu loaded [ INFO] [1509354601.263345305]: Plugin imu initialized [ INFO] [1509354601.263646428]: Plugin local_position loaded [ INFO] [1509354601.267964906]: Plugin local_position initialized [ INFO] [1509354601.268152309]: Plugin manual_control loaded [ INFO] [1509354601.268723752]: Plugin manual_control initialized [ INFO] [1509354601.268830602]: Plugin mocap_pose_estimate loaded [ INFO] [1509354601.272102227]: Plugin mocap_pose_estimate initialized [ INFO] [1509354601.272246216]: Plugin odom loaded [ INFO] [1509354601.275245183]: Plugin odom initialized [ INFO] [1509354601.275508318]: Plugin param loaded [ INFO] [1509354601.277815006]: Plugin param initialized [ INFO] [1509354601.277918757]: Plugin px4flow loaded [ INFO] [1509354601.282288875]: Plugin px4flow initialized [ INFO] [1509354601.282329655]: Plugin rangefinder blacklisted [ INFO] [1509354601.282565174]: Plugin rc_io loaded [ INFO] [1509354601.285849491]: Plugin rc_io initialized [ INFO] [1509354601.285883328]: Plugin safety_area blacklisted [ INFO] [1509354601.286081906]: Plugin setpoint_accel loaded [ INFO] [1509354601.289582399]: Plugin setpoint_accel initialized [ INFO] [1509354601.289869833]: Plugin setpoint_attitude loaded [ INFO] [1509354601.304382814]: Plugin setpoint_attitude initialized [ INFO] [1509354601.304559135]: Plugin setpoint_position loaded [ INFO] [1509354601.311833674]: Plugin setpoint_position initialized [ INFO] [1509354601.312039466]: Plugin setpoint_raw loaded [ INFO] [1509354601.327968414]: Plugin setpoint_raw initialized [ INFO] [1509354601.328172333]: Plugin setpoint_velocity loaded [ INFO] [1509354601.334873630]: Plugin setpoint_velocity initialized [ INFO] [1509354601.335150301]: Plugin sys_status loaded [ INFO] [1509354601.343410021]: Plugin sys_status initialized [ INFO] [1509354601.343588172]: Plugin sys_time loaded [ INFO] [1509354601.347681688]: TM: Timesync mode: MAVLINK [ INFO] [1509354601.348580549]: Plugin sys_time initialized [ INFO] [1509354601.348768263]: Plugin vfr_hud loaded [ INFO] [1509354601.350216732]: Plugin vfr_hud initialized [ INFO] [1509354601.350243626]: Plugin vibration blacklisted [ INFO] [1509354601.350377629]: Plugin vision_pose_estimate loaded [ INFO] [1509354601.361003558]: Plugin vision_pose_estimate initialized [ INFO] [1509354601.361216683]: Plugin vision_speed_estimate loaded [ INFO] [1509354601.365901167]: Plugin vision_speed_estimate initialized [ INFO] [1509354601.366080919]: Plugin waypoint loaded [ INFO] [1509354601.371300429]: Plugin waypoint initialized [ INFO] [1509354601.371354304]: Autostarting mavlink via USB on PX4 [ INFO] [1509354601.371496138]: Built-in SIMD instructions: SSE, SSE2 [ INFO] [1509354601.371520420]: Built-in MAVLink package version: 2017.9.9 [ INFO] [1509354601.371544721]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta [ INFO] [1509354601.371565378]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [ INFO] [1509354601.407577285]: Finished loading Gazebo ROS API Plugin. [ INFO] [1509354601.407951189]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting... [INFO] [1509354601.829435, 0.000000]: Calling service /gazebo/spawn_sdf_model [INFO] [1509354601.928632, 786.962000]: Spawn status: SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name iris [spawn_model-9] process has finished cleanly log file: /home/lkumarbe/.ros/log/1a38f564-bd52-11e7-9ac9-1c1b0d1ecbab/spawn_model-9*.log [ INFO] [1509354602.286673080, 786.962000000]: Camera Plugin: The 'robotNamespace' param did not exit [ INFO] [1509354602.289690163, 786.962000000]: Camera Plugin (ns = ) , set to "" [ INFO] [1509354602.502154407, 786.962000000]: Laser Plugin: The 'robotNamespace' param did not exit [ INFO] [1509354602.502602240, 786.962000000]: Starting Laser Plugin (ns = ) [ INFO] [1509354602.505104090, 786.962000000]: Laser Plugin (ns = ) , set to "" [ INFO] [1509354602.674359639, 786.974000000]: ARGS: /home/lkumarbe/catkin_ws/src/avoidance/node/resource/random_goals [ INFO] [1509354602.674509122, 786.974000000]: Read 0 waypoints. [ INFO] [1509354602.683665032, 786.982000000]: waitForService: Service [/gazebo/set_physics_properties] is now available. [ INFO] [1509354602.708586089, 787.008000000]: Physics dynamic reconfigure ready. INFO [simulator] Got initial simulation data, running sim.. INFO [pwm_out_sim] MODE_8PWM pwm_out_sim CDev::init pwm_out_sim CDev::register_driver /dev/pwm_output0 pwm_out_sim Registered DEV /dev/pwm_output0 INFO [tone_alarm] startup pwm_out_sim CDev::open pwm_out_sim CDev::open_first 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] [1509354603.800608389, 788.078000000]: udp0: Remote address: 127.0.0.1:14557 [ INFO] [1509354603.800850761, 788.078000000]: IMU: High resolution IMU detected! INFO [logger] Opened log file: rootfs/fs/microsd/log/2017-10-30/09_10_03.ulg INFO [mavlink] partner IP: 127.0.0.1 [ WARN] [1509354603.838464005, 788.116000000]: TM: Clock skew detected (-787.918112000 s). Hard syncing clocks. ERROR [mavlink] [timesync] Hard setting offset. INFO [lib__ecl] EKF aligned, (pressure height, IMU buf: 15, OBS buf: 14) [ INFO] [1509354604.680854719, 788.958000000]: IMU: Attitude quaternion IMU detected! [ INFO] [1509354604.746895528, 789.022000000]: Creating 1 swatches [ INFO] [1509354604.750586757, 789.024000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot [ INFO] [1509354604.751749524, 789.026000000]: IMU: High resolution IMU detected! [ INFO] [1509354604.751982238, 789.026000000]: IMU: Attitude quaternion IMU detected! [ INFO] [1509354604.907034161, 789.182000000]: Creating 1 swatches [ INFO] [1509354604.971182966, 789.246000000]: Creating 1 swatches [ INFO] [1509354605.067611586, 789.342000000]: Creating 1 swatches [ INFO] [1509354605.195040031, 789.466000000]: Creating 1 swatches [ INFO] [1509354605.226862573, 789.498000000]: Creating 1 swatches [ INFO] [1509354605.322637001, 789.592000000]: Creating 1 swatches [ INFO] [1509354605.418715352, 789.688000000]: Creating 1 swatches [ INFO] [1509354605.758113732, 790.026000000]: VER: 1.1: Capabilities 0x000000000000e4ef [ INFO] [1509354605.758151317, 790.026000000]: VER: 1.1: Flight software: 01060500 (000000009B97976D) [ INFO] [1509354605.758169959, 790.026000000]: VER: 1.1: Middleware software: 01060500 (000000009B97976D) [ INFO] [1509354605.758198829, 790.026000000]: VER: 1.1: OS software: 040400ff (0000000000000000) [ INFO] [1509354605.758211918, 790.026000000]: VER: 1.1: Board hardware: 00000001 [ INFO] [1509354605.758223724, 790.026000000]: VER: 1.1: VID/PID: 0000:0000 [ INFO] [1509354605.758234652, 790.026000000]: VER: 1.1: UID: 0000000100000002 [ WARN] [1509354605.806659204, 790.074000000]: CMD: Unexpected command 520, result 0 [ WARN] [1509354607.937743979, 792.190000000]: Transformation not available (/world to /camera_link INFO [lib__ecl] EKF GPS checks passed (WGS-84 origin set) INFO [lib__ecl] EKF commencing GPS fusion INFO [commander] home: 47.3977420, 8.5455936, 488.09 INFO [tone_alarm] home_set INFO [tone_alarm] positive [ WARN] [1509354618.453423535, 802.620000000]: CMD: Unexpected command 176, result 0 [ INFO] [1509354619.873971383, 804.024000000]: WP: mission received INFO [commander] home: 47.3977421, 8.5455934, 488.01 INFO [tone_alarm] positive INFO [commander] home: 47.3977421, 8.5455934, 488.01 INFO [tone_alarm] arming INFO [commander] Takeoff detected [ INFO] [1509354622.250817995, 806.384000000]: Creating 1 swatches [ INFO] [1509354623.178536002, 807.306000000]: Creating 1 swatches [ INFO] [1509354637.412826779, 821.436000000]: Setting goal: Frame:world, Position(31.191, 6.393, 0.000), Orientation(0.000, 0.000, 0.027, 1.000) = Angle: 0.055 [ INFO] [1509354637.412998294, 821.436000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354637.413072730, 821.436000000]: OctoMap memory usage: 0.029 MB [ INFO] [1509354637.413221440, 821.436000000]: Planning a path from (0,0,3) to (31,6,3) [ INFO] [1509354637.413241057, 821.436000000]: curr_pos_: 0.46,0.50,3.73 s: 0.50,0.50,3.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 168 2 39 0 (cost: 193.61, dist: 33.49, risk: 40.13, smooth: 120.00) SpeedNode 248 1.25 39 0 (cost: 86.79, dist: 32.32, risk: 39.29, smooth: 15.18) SpeedNode 231 1.06 41 0 (cost: 84.85, dist: 32.32, risk: 39.29, smooth: 13.23) Total time: 29.27 ms [ INFO] [1509354637.867069752, 821.886000000]: Creating 1 swatches [ INFO] [1509354638.795392535, 822.806000000]: Creating 1 swatches [ INFO] [1509354638.891113097, 822.902000000]: Creating 1 swatches [ INFO] [1509354639.883091844, 823.890000000]: Creating 1 swatches ERROR [mavlink] [timesync] Hard setting offset. [ INFO] [1509354640.685667490, 824.686000000]: Risk increase [ INFO] [1509354640.685735070, 824.686000000]: Path is bad, planning a new path [ INFO] [1509354640.685829901, 824.686000000]: OctoMap memory usage: 0.034 MB [ INFO] [1509354640.685875378, 824.686000000]: Planning a path from (3,1,3) to (31,6,3) [ INFO] [1509354640.685898870, 824.686000000]: curr_pos_: 2.84,1.71,4.00 s: 3.50,1.50,3.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 166 2 36 0 (cost: 219.40, dist: 30.07, risk: 39.33, smooth: 105.00) SpeedNode 271 1.25 47 0 (cost: 98.47, dist: 46.73, risk: 14.64, smooth: 27.10) [ INFO] [1509354640.714632918, 824.716000000]: Creating 1 swatches SpeedNode 267 1.06 47 0 (cost: 95.58, dist: 46.65, risk: 15.38, smooth: 23.55) Total time: 35.66 ms [ INFO] [1509354640.907237614, 824.908000000]: Creating 1 swatches [ INFO] [1509354642.026350411, 826.024000000]: Creating 1 swatches [ INFO] [1509354643.050968411, 827.042000000]: Creating 1 swatches [ INFO] [1509354643.979255084, 827.964000000]: Creating 1 swatches [ INFO] [1509354644.714743999, 828.698000000]: Creating 1 swatches [ INFO] [1509354645.483069030, 829.464000000]: Creating 1 swatches [ INFO] [1509354646.186969075, 830.166000000]: Creating 1 swatches [ INFO] [1509354647.530684264, 831.508000000]: Creating 1 swatches [ INFO] [1509354647.659003530, 831.634000000]: Creating 1 swatches [ INFO] [1509354648.074970425, 832.048000000]: Creating 1 swatches [ INFO] [1509354648.215746978, 832.190000000]: Risk increase [ INFO] [1509354648.215781801, 832.190000000]: Path is bad, planning a new path [ INFO] [1509354648.215857233, 832.190000000]: OctoMap memory usage: 0.052 MB [ INFO] [1509354648.215882147, 832.190000000]: Planning a path from (13,2,6) to (31,6,3) [ INFO] [1509354648.215892946, 832.190000000]: curr_pos_: 12.46,1.96,6.50 s: 13.50,2.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 177 2 27 0 (cost: 178.18, dist: 22.66, risk: 16.59, smooth: 131.97) SpeedNode 350 1.25 30 0 (cost: 44.75, dist: 21.76, risk: 15.20, smooth: 7.79) SpeedNode 272 1.06 33 0 (cost: 44.75, dist: 21.76, risk: 15.20, smooth: 7.79) Total time: 27.76 ms [ INFO] [1509354648.682960007, 832.656000000]: Creating 1 swatches [ INFO] [1509354648.718200800, 832.692000000]: Risk increase [ INFO] [1509354648.718281298, 832.692000000]: Path is bad, planning a new path [ INFO] [1509354648.718436908, 832.692000000]: OctoMap memory usage: 0.056 MB [ INFO] [1509354648.718481956, 832.692000000]: Planning a path from (13,2,6) to (31,6,3) [ INFO] [1509354648.718507599, 832.692000000]: curr_pos_: 13.21,2.08,6.50 s: 13.50,2.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 177 2 42 0 (cost: 186.45, dist: 23.49, risk: 17.96, smooth: 105.00) SpeedNode 242 1.25 99 0 (cost: 66.25, dist: 22.95, risk: 16.00, smooth: 27.30) SpeedNode 236 1.06 96 0 (cost: 66.25, dist: 22.95, risk: 16.00, smooth: 27.30) Total time: 58.64 ms [ INFO] [1509354649.195156819, 833.166000000]: Creating 1 swatches [ INFO] [1509354649.273609060, 833.246000000]: Risk increase [ INFO] [1509354649.273643757, 833.246000000]: Path is bad, planning a new path [ INFO] [1509354649.273719837, 833.246000000]: OctoMap memory usage: 0.060 MB [ INFO] [1509354649.273745169, 833.246000000]: Planning a path from (14,2,6) to (31,6,3) [ INFO] [1509354649.273780392, 833.246000000]: curr_pos_: 13.94,2.33,6.50 s: 14.50,2.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 152 2 40 0 (cost: 181.56, dist: 23.31, risk: 13.25, smooth: 125.00) SpeedNode 219 1.25 91 0 (cost: 72.78, dist: 22.63, risk: 8.10, smooth: 42.04) SpeedNode 178 1.06 95 0 (cost: 72.78, dist: 22.63, risk: 8.10, smooth: 42.04) Total time: 45.68 ms [ INFO] [1509354649.386696017, 833.358000000]: Creating 1 swatches [ INFO] [1509354649.773416867, 833.744000000]: Risk increase [ INFO] [1509354649.773495970, 833.744000000]: Path is bad, planning a new path [ INFO] [1509354649.773715856, 833.744000000]: OctoMap memory usage: 0.064 MB [ INFO] [1509354649.773796806, 833.744000000]: Planning a path from (15,2,6) to (31,6,3) [ INFO] [1509354649.773852037, 833.746000000]: curr_pos_: 14.52,2.66,6.50 s: 15.50,2.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 225 2 46 0 (cost: 136.76, dist: 23.14, risk: 8.62, smooth: 105.00) SpeedNode 204 1.25 136 0 (cost: 62.88, dist: 23.10, risk: 9.72, smooth: 26.70) SpeedNode 187 1.06 141 0 (cost: 62.88, dist: 23.10, risk: 9.72, smooth: 26.70) Total time: 70.83 ms [ INFO] [1509354650.282661038, 834.252000000]: Creating 1 swatches [ INFO] [1509354652.618665015, 836.580000000]: Creating 1 swatches [ INFO] [1509354653.290837409, 837.250000000]: Creating 1 swatches [ INFO] [1509354653.642717815, 837.602000000]: Creating 1 swatches [ INFO] [1509354653.823104160, 837.782000000]: Risk increase [ INFO] [1509354653.823134001, 837.782000000]: Path is bad, planning a new path [ INFO] [1509354653.823207756, 837.782000000]: OctoMap memory usage: 0.078 MB [ INFO] [1509354653.823233643, 837.782000000]: Planning a path from (20,0,6) to (31,6,3) [ INFO] [1509354653.823251511, 837.782000000]: curr_pos_: 20.13,1.01,6.46 s: 20.50,0.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 143 2 34 0 (cost: 241.97, dist: 20.24, risk: 16.73, smooth: 185.00) [ INFO] [1509354653.835503189, 837.794000000]: Creating 1 swatches SpeedNode 250 1.25 113 0 (cost: 89.30, dist: 21.01, risk: 13.63, smooth: 54.66) SpeedNode 212 1.06 115 0 (cost: 89.30, dist: 21.01, risk: 13.63, smooth: 54.66) Total time: 61.26 ms [ INFO] [1509354654.363515318, 838.322000000]: Risk increase [ INFO] [1509354654.363612118, 838.322000000]: Path is bad, planning a new path [ INFO] [1509354654.363758942, 838.322000000]: OctoMap memory usage: 0.081 MB [ INFO] [1509354654.363815154, 838.322000000]: Planning a path from (21,0,6) to (31,6,3) [ INFO] [1509354654.363847430, 838.322000000]: curr_pos_: 20.93,0.66,6.46 s: 21.50,0.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 154 2 24 0 (cost: 273.90, dist: 19.83, risk: 9.07, smooth: 205.00) SpeedNode 247 1.25 88 0 (cost: 78.82, dist: 19.87, risk: 8.64, smooth: 43.35) SpeedNode 209 1.06 89 0 (cost: 78.82, dist: 19.87, risk: 8.64, smooth: 43.35) Total time: 47.72 ms [ INFO] [1509354654.538715515, 838.498000000]: Creating 1 swatches [ INFO] [1509354654.874348183, 838.832000000]: Risk increase [ INFO] [1509354654.874392246, 838.832000000]: Path is bad, planning a new path [ INFO] [1509354654.874625837, 838.832000000]: OctoMap memory usage: 0.082 MB [ INFO] [1509354654.874707657, 838.832000000]: Planning a path from (22,0,6) to (31,6,3) [ INFO] [1509354654.874743327, 838.832000000]: curr_pos_: 21.61,0.32,6.47 s: 22.50,0.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 154 2 32 0 (cost: 246.54, dist: 36.83, risk: 9.71, smooth: 195.00) SpeedNode 139 1.25 1 0 ===== Half-way path ====== [ INFO] [1509354654.881186146, 838.840000000]: ========== Set goal : (31,0,9) ========== [ INFO] [1509354654.881757604, 838.840000000]: OctoMap memory usage: 0.082 MB [ INFO] [1509354654.881781251, 838.840000000]: Planning a path from (22,0,6) to (31,0,9) [ INFO] [1509354654.881793591, 838.840000000]: curr_pos_: 21.61,0.32,6.47 s: 22.50,0.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 186 2 12 0 (cost: 64.23, dist: 23.41, risk: 5.82, smooth: 30.00) SpeedNode 91 1.25 1 0 ===== Half-way path ====== [ INFO] [1509354654.884648867, 838.842000000]: ========== Set goal : (24,-1,9) ========== [ INFO] [1509354654.884737374, 838.842000000]: OctoMap memory usage: 0.082 MB [ INFO] [1509354654.884763157, 838.842000000]: Planning a path from (22,0,6) to (24,-1,9) [ INFO] [1509354654.884779679, 838.842000000]: curr_pos_: 21.61,0.32,6.47 s: 22.50,0.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 164 2 4 0 (cost: 36.91, dist: 16.41, risk: 5.49, smooth: 10.00) SpeedNode 91 1.25 1 0 Total time: 1.29 ms Total time: 4.55 ms Total time: 12.44 ms [ INFO] [1509354655.275423335, 839.232000000]: Creating 1 swatches [ INFO] [1509354656.367578811, 840.322000000]: ========== Set goal : (31,0,9) ========== [ INFO] [1509354656.367842223, 840.322000000]: OctoMap memory usage: 0.085 MB [ INFO] [1509354656.367918727, 840.322000000]: Planning a path from (23,-1,9) to (31,0,9) [ INFO] [1509354656.367948050, 840.322000000]: curr_pos_: 23.12,-0.04,8.36 s: 23.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 248 2 7 0 (cost: 97.31, dist: 7.00, risk: 0.31, smooth: 45.00) SpeedNode 21 1.25 1 0 Total time: 3.19 ms [ INFO] [1509354656.370619614, 840.326000000]: Reached current goal (31,0,9), 1 goals left [ INFO] [1509354656.370651847, 840.326000000]: Actual travel distance: 29.98 Actual energy usage: 72.70 [ INFO] [1509354656.395503591, 840.350000000]: Creating 1 swatches [ INFO] [1509354657.355204259, 841.308000000]: Creating 1 swatches [ INFO] [1509354658.058567025, 842.012000000]: Creating 1 swatches [ INFO] [1509354658.634511640, 842.586000000]: Creating 1 swatches [ INFO] [1509354658.966027908, 842.918000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354658.966172786, 842.918000000]: OctoMap memory usage: 0.095 MB [ INFO] [1509354658.966212452, 842.918000000]: Planning a path from (27,-1,9) to (31,6,3) [ INFO] [1509354658.966225310, 842.918000000]: curr_pos_: 26.78,-0.68,9.42 s: 27.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 137 2 73 0 (cost: 109.54, dist: 22.07, risk: 17.47, smooth: 70.00) SpeedNode 198 1.25 206 0 (cost: 70.68, dist: 27.06, risk: 18.29, smooth: 25.33) SpeedNode 177 1.06 206 0 (cost: 70.68, dist: 27.06, risk: 18.29, smooth: 25.33) Total time: 92.08 ms [ INFO] [1509354659.338508254, 843.290000000]: Creating 1 swatches [ INFO] [1509354659.440221978, 843.392000000]: Risk increase [ INFO] [1509354659.440267763, 843.392000000]: Path is bad, planning a new path [ INFO] [1509354659.440357633, 843.392000000]: OctoMap memory usage: 0.096 MB [ INFO] [1509354659.440383036, 843.392000000]: Planning a path from (28,-1,9) to (31,6,3) [ INFO] [1509354659.440401748, 843.392000000]: curr_pos_: 27.53,-0.68,9.44 s: 28.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 137 2 112 0 (cost: 131.90, dist: 23.07, risk: 18.83, smooth: 90.00) SpeedNode 184 1.25 212 0 (cost: 76.31, dist: 27.48, risk: 18.50, smooth: 30.33) SpeedNode 173 1.06 210 0 (cost: 76.31, dist: 27.48, risk: 18.50, smooth: 30.33) Total time: 95.24 ms [ INFO] [1509354660.004556675, 843.956000000]: Risk increase [ INFO] [1509354660.004631742, 843.956000000]: Path is bad, planning a new path [ INFO] [1509354660.004811805, 843.956000000]: OctoMap memory usage: 0.097 MB [ INFO] [1509354660.004856529, 843.956000000]: Planning a path from (29,-1,9) to (31,6,3) [ INFO] [1509354660.004879220, 843.956000000]: curr_pos_: 28.35,-0.66,9.45 s: 29.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 139 2 73 0 (cost: 170.59, dist: 22.07, risk: 58.52, smooth: 90.00) SpeedNode 181 1.25 210 0 (cost: 87.23, dist: 22.71, risk: 34.13, smooth: 30.40) SpeedNode 176 1.06 208 0 (cost: 87.23, dist: 22.71, risk: 34.13, smooth: 30.40) Total time: 89.68 ms [ INFO] [1509354660.555043498, 844.508000000]: Risk increase [ INFO] [1509354660.555136578, 844.508000000]: Path is bad, planning a new path [ INFO] [1509354660.555347187, 844.508000000]: OctoMap memory usage: 0.098 MB [ INFO] [1509354660.555402227, 844.508000000]: Planning a path from (29,-1,9) to (31,6,3) [ INFO] [1509354660.555438316, 844.508000000]: curr_pos_: 29.15,-0.62,9.47 s: 29.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 214 2 121 0 (cost: 174.06, dist: 24.07, risk: 19.99, smooth: 130.00) SpeedNode 254 1.25 305 0 (cost: 85.49, dist: 25.54, risk: 3.97, smooth: 55.98) SpeedNode 163 1.06 304 0 (cost: 85.49, dist: 25.54, risk: 3.97, smooth: 55.98) Total time: 158.99 ms [ INFO] [1509354661.691781755, 845.642000000]: Risk increase [ INFO] [1509354661.691861729, 845.642000000]: Path is bad, planning a new path [ INFO] [1509354661.692230465, 845.642000000]: OctoMap memory usage: 0.099 MB [ INFO] [1509354661.692279603, 845.642000000]: Planning a path from (31,-1,9) to (31,6,3) [ INFO] [1509354661.692303911, 845.642000000]: curr_pos_: 30.82,-0.52,9.47 s: 31.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 143 2 83 0 (cost: 203.98, dist: 23.49, risk: 25.50, smooth: 150.00) SpeedNode 181 1.25 31 0 (cost: 104.18, dist: 28.81, risk: 31.36, smooth: 44.01) SpeedNode 156 1.06 33 0 (cost: 104.18, dist: 28.81, risk: 31.36, smooth: 44.01) Total time: 25.46 ms [ INFO] [1509354662.171267196, 846.122000000]: Risk increase [ INFO] [1509354662.171296941, 846.122000000]: Path is bad, planning a new path [ INFO] [1509354662.171380777, 846.122000000]: OctoMap memory usage: 0.099 MB [ INFO] [1509354662.171400543, 846.122000000]: Planning a path from (32,-1,9) to (31,6,3) [ INFO] [1509354662.171412306, 846.122000000]: curr_pos_: 31.63,-0.49,9.48 s: 32.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 136 2 133 0 (cost: 1063.16, dist: 25.31, risk: 42.85, smooth: 570.00) SpeedNode 243 1.25 27 0 (cost: 120.92, dist: 22.36, risk: 58.97, smooth: 32.62) SpeedNode 218 1.06 27 0 (cost: 120.92, dist: 22.36, risk: 58.97, smooth: 32.62) Total time: 33.37 ms [ INFO] [1509354662.677920380, 846.626000000]: Risk increase [ INFO] [1509354662.677974691, 846.626000000]: Path is bad, planning a new path [ INFO] [1509354662.678171378, 846.626000000]: OctoMap memory usage: 0.099 MB [ INFO] [1509354662.678216339, 846.626000000]: Planning a path from (33,-1,9) to (31,6,3) [ INFO] [1509354662.678242450, 846.626000000]: curr_pos_: 32.37,-0.45,9.49 s: 33.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 146 2 108 0 (cost: 1049.92, dist: 23.49, risk: 41.43, smooth: 560.00) SpeedNode 227 1.25 24 0 (cost: 162.07, dist: 21.94, risk: 81.85, smooth: 51.31) SpeedNode 215 1.06 23 0 (cost: 169.97, dist: 22.89, risk: 85.85, smooth: 54.27) Total time: 29.26 ms [ INFO] [1509354663.182451963, 847.130000000]: Risk increase [ INFO] [1509354663.182590743, 847.130000000]: Path is bad, planning a new path [ INFO] [1509354663.182837193, 847.132000000]: OctoMap memory usage: 0.099 MB [ INFO] [1509354663.182913744, 847.132000000]: Planning a path from (33,-1,9) to (31,6,3) [ INFO] [1509354663.182965636, 847.132000000]: curr_pos_: 33.09,-0.50,9.50 s: 33.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 141 2 123 0 (cost: 724.06, dist: 26.31, risk: 27.74, smooth: 410.00) SpeedNode 180 1.25 1877 0 ===== Half-way path ====== [ INFO] [1509354663.537828383, 847.486000000]: ========== Set goal : (34,-3,3) ========== [ INFO] [1509354663.539623273, 847.488000000]: OctoMap memory usage: 0.099 MB [ INFO] [1509354663.539668791, 847.488000000]: Planning a path from (33,-1,9) to (34,-3,3) [ INFO] [1509354663.539685257, 847.488000000]: curr_pos_: 33.09,-0.50,9.50 s: 33.50,-0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 277 2 6 0 (cost: 140.76, dist: 6.41, risk: 4.35, smooth: 65.00) SpeedNode 193 1.25 13 0 (cost: 42.24, dist: 7.00, risk: 25.24, smooth: 5.00) SpeedNode 181 1.06 13 0 (cost: 42.24, dist: 7.00, risk: 25.24, smooth: 5.00) Total time: 8.11 ms Total time: 378.18 ms [ INFO] [1509354664.554471580, 848.502000000]: Creating 1 swatches [ INFO] [1509354664.906414494, 848.852000000]: Creating 1 swatches [ INFO] [1509354665.997152037, 849.942000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354665.997424301, 849.942000000]: OctoMap memory usage: 0.103 MB [ INFO] [1509354665.997506151, 849.942000000]: Planning a path from (35,-2,7) to (31,6,3) [ INFO] [1509354665.997540625, 849.942000000]: curr_pos_: 35.42,-1.44,7.47 s: 35.50,-1.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 156 2 78 0 (cost: 172.92, dist: 17.07, risk: 20.85, smooth: 130.00) SpeedNode 180 1.25 390 0 (cost: 52.11, dist: 19.30, risk: 7.16, smooth: 25.65) SpeedNode 168 1.06 380 0 (cost: 52.11, dist: 19.30, risk: 7.16, smooth: 25.65) Total time: 153.37 ms [ INFO] [1509354666.154897446, 850.100000000]: Creating 1 swatches [ INFO] [1509354666.410781643, 850.356000000]: Creating 1 swatches [ INFO] [1509354666.891248977, 850.836000000]: Creating 1 swatches [ INFO] [1509354669.141308590, 853.084000000]: Risk increase [ INFO] [1509354669.141334578, 853.084000000]: Path is bad, planning a new path [ INFO] [1509354669.141448485, 853.084000000]: OctoMap memory usage: 0.108 MB [ INFO] [1509354669.141471806, 853.084000000]: Planning a path from (38,-2,7) to (31,6,3) [ INFO] [1509354669.141483540, 853.084000000]: curr_pos_: 38.13,-1.64,7.54 s: 38.50,-1.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 153 2 40 0 (cost: 420.89, dist: 16.66, risk: 19.23, smooth: 235.00) SpeedNode 274 1.25 20 0 (cost: 70.27, dist: 17.36, risk: 6.61, smooth: 35.98) SpeedNode 236 1.06 19 0 (cost: 70.27, dist: 17.36, risk: 6.61, smooth: 35.98) Total time: 19.24 ms [ INFO] [1509354669.644577858, 853.584000000]: Risk increase [ INFO] [1509354669.644647987, 853.584000000]: Path is bad, planning a new path [ INFO] [1509354669.644884116, 853.584000000]: OctoMap memory usage: 0.109 MB [ INFO] [1509354669.644923098, 853.584000000]: Planning a path from (38,-1,7) to (31,6,3) [ INFO] [1509354669.644939475, 853.584000000]: curr_pos_: 38.30,-1.28,7.57 s: 38.50,-0.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 146 2 177 0 (cost: 132.00, dist: 19.66, risk: 7.34, smooth: 105.00) SpeedNode 232 1.25 21 0 (cost: 75.39, dist: 19.30, risk: 7.08, smooth: 45.65) SpeedNode 270 1.06 21 0 (cost: 75.39, dist: 19.30, risk: 7.08, smooth: 45.65) Total time: 39.87 ms [ INFO] [1509354670.153072329, 854.094000000]: Risk increase [ INFO] [1509354670.153121865, 854.094000000]: Path is bad, planning a new path [ INFO] [1509354670.153384890, 854.096000000]: OctoMap memory usage: 0.111 MB [ INFO] [1509354670.153437679, 854.096000000]: Planning a path from (38,-1,7) to (31,6,3) [ INFO] [1509354670.153462514, 854.096000000]: curr_pos_: 38.54,-0.94,7.61 s: 38.50,-0.50,7.50 Search iter_time overest num_iter path_cost [ INFO] [1509354670.155331607, 854.096000000]: Creating 1 swatches NodeWithoutSmooth 140 2 211 0 (cost: 240.50, dist: 21.66, risk: 23.85, smooth: 130.00) SpeedNode 254 1.25 41 0 (cost: 116.79, dist: 22.42, risk: 8.06, smooth: 62.95) SpeedNode 219 1.06 44 0 (cost: 116.79, dist: 22.42, risk: 8.06, smooth: 62.95) Total time: 52.76 ms [ INFO] [1509354670.314320382, 854.256000000]: Creating 1 swatches [ INFO] [1509354670.474533154, 854.416000000]: Creating 1 swatches [ INFO] [1509354670.794445106, 854.736000000]: Creating 1 swatches [ INFO] [1509354671.642958477, 855.582000000]: Risk increase [ INFO] [1509354671.643040452, 855.584000000]: Path is bad, planning a new path [ INFO] [1509354671.643474259, 855.584000000]: OctoMap memory usage: 0.115 MB [ INFO] [1509354671.643542670, 855.584000000]: Planning a path from (40,0,7) to (31,6,3) [ INFO] [1509354671.643571703, 855.584000000]: curr_pos_: 39.48,0.27,7.68 s: 40.50,0.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 131 2 276 0 (cost: 538.85, dist: 24.90, risk: 24.46, smooth: 409.74) SpeedNode 240 1.25 52 0 (cost: 86.36, dist: 27.31, risk: 9.39, smooth: 49.66) SpeedNode 241 1.06 81 0 (cost: 86.36, dist: 27.31, risk: 9.39, smooth: 49.66) Total time: 73.15 ms [ INFO] [1509354671.850386460, 855.790000000]: Creating 1 swatches [ INFO] [1509354672.138885748, 856.078000000]: Risk increase [ INFO] [1509354672.138934312, 856.078000000]: Path is bad, planning a new path [ INFO] [1509354672.139144820, 856.078000000]: OctoMap memory usage: 0.117 MB [ INFO] [1509354672.139187799, 856.078000000]: Planning a path from (40,1,7) to (31,6,3) [ INFO] [1509354672.139215544, 856.078000000]: curr_pos_: 39.86,0.77,7.69 s: 40.50,1.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 139 2 217 0 (cost: 348.73, dist: 23.07, risk: 35.66, smooth: 270.00) SpeedNode 252 1.25 97 0 (cost: 81.68, dist: 26.26, risk: 9.11, smooth: 46.31) SpeedNode 276 1.06 99 0 (cost: 81.68, dist: 26.26, risk: 9.11, smooth: 46.31) Total time: 86.05 ms [ INFO] [1509354672.645690417, 856.584000000]: Risk increase [ INFO] [1509354672.645737991, 856.586000000]: Path is bad, planning a new path [ INFO] [1509354672.645887372, 856.586000000]: OctoMap memory usage: 0.118 MB [ INFO] [1509354672.645920029, 856.586000000]: Planning a path from (40,1,7) to (31,6,3) [ INFO] [1509354672.645937400, 856.586000000]: curr_pos_: 40.28,1.23,7.71 s: 40.50,1.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 155 2 257 0 (cost: 397.71, dist: 24.49, risk: 23.22, smooth: 330.00) SpeedNode 249 1.25 155 0 (cost: 92.94, dist: 26.77, risk: 9.14, smooth: 53.02) SpeedNode 282 1.06 99 0 (cost: 84.20, dist: 28.26, risk: 9.64, smooth: 46.31) Total time: 112.75 ms [ INFO] [1509354673.182122162, 857.120000000]: Risk increase [ INFO] [1509354673.182161474, 857.120000000]: Path is bad, planning a new path [ INFO] [1509354673.182287416, 857.120000000]: OctoMap memory usage: 0.119 MB [ INFO] [1509354673.182315589, 857.120000000]: Planning a path from (41,2,7) to (31,6,3) [ INFO] [1509354673.182332453, 857.120000000]: curr_pos_: 40.69,1.79,7.72 s: 41.50,2.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 141 2 126 0 (cost: 350.57, dist: 23.90, risk: 21.67, smooth: 260.00) SpeedNode 254 1.25 160 0 (cost: 83.60, dist: 27.67, risk: 9.62, smooth: 46.31) SpeedNode 234 1.06 179 0 (cost: 88.04, dist: 27.90, risk: 10.48, smooth: 49.66) Total time: 107.17 ms [ INFO] [1509354673.760210734, 857.696000000]: Risk increase [ INFO] [1509354673.760243424, 857.696000000]: Path is bad, planning a new path [ INFO] [1509354673.760405231, 857.696000000]: OctoMap memory usage: 0.120 MB [ INFO] [1509354673.760448963, 857.696000000]: Planning a path from (41,2,7) to (31,6,3) [ INFO] [1509354673.760464378, 857.696000000]: curr_pos_: 41.14,2.46,7.72 s: 41.50,2.50,7.50 Search iter_time overest num_iter path_cost [ INFO] [1509354673.771147682, 857.706000000]: Creating 1 swatches NodeWithoutSmooth 146 2 199 0 (cost: 252.86, dist: 25.31, risk: 22.54, smooth: 200.00) SpeedNode 218 1.25 97 0 (cost: 99.49, dist: 24.96, risk: 23.88, smooth: 50.65) SpeedNode 201 1.06 99 0 (cost: 99.49, dist: 24.96, risk: 23.88, smooth: 50.65) Total time: 76.13 ms [ INFO] [1509354674.474453202, 858.410000000]: Creating 1 swatches [ INFO] [1509354674.731054316, 858.666000000]: Creating 1 swatches [ INFO] [1509354674.759463071, 858.694000000]: Risk increase [ INFO] [1509354674.759491940, 858.694000000]: Path is bad, planning a new path [ INFO] [1509354674.759676184, 858.694000000]: OctoMap memory usage: 0.124 MB [ INFO] [1509354674.759709702, 858.694000000]: Planning a path from (41,4,7) to (31,6,3) [ INFO] [1509354674.759723541, 858.694000000]: curr_pos_: 41.72,3.55,7.64 s: 41.50,4.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 140 2 236 0 (cost: 383.46, dist: 24.73, risk: 43.73, smooth: 190.00) SpeedNode 207 1.25 106 0 (cost: 101.97, dist: 24.73, risk: 25.32, smooth: 51.92) SpeedNode 226 1.06 109 0 (cost: 101.97, dist: 24.73, risk: 25.32, smooth: 51.92) Total time: 85.49 ms [ INFO] [1509354675.311278433, 859.246000000]: Risk increase [ INFO] [1509354675.311310714, 859.246000000]: Path is bad, planning a new path [ INFO] [1509354675.311491468, 859.246000000]: OctoMap memory usage: 0.125 MB [ INFO] [1509354675.311519226, 859.246000000]: Planning a path from (41,4,7) to (31,6,3) [ INFO] [1509354675.311535007, 859.246000000]: curr_pos_: 41.84,4.38,7.62 s: 41.50,4.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 140 2 346 0 (cost: 669.23, dist: 24.49, risk: 24.74, smooth: 365.00) SpeedNode 232 1.25 191 0 (cost: 76.05, dist: 30.42, risk: 9.65, smooth: 35.98) SpeedNode 221 1.06 194 0 (cost: 76.62, dist: 30.78, risk: 9.86, smooth: 35.98) Total time: 142.44 ms [ INFO] [1509354675.466928855, 859.402000000]: Creating 1 swatches [ INFO] [1509354675.898476796, 859.834000000]: Risk increase [ INFO] [1509354675.898509312, 859.834000000]: Path is bad, planning a new path [ INFO] [1509354675.898716844, 859.834000000]: OctoMap memory usage: 0.127 MB [ INFO] [1509354675.898754509, 859.834000000]: Planning a path from (41,5,7) to (31,6,3) [ INFO] [1509354675.898771045, 859.834000000]: curr_pos_: 41.89,5.20,7.60 s: 41.50,5.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 146 2 366 0 (cost: 1310.24, dist: 25.49, risk: 24.75, smooth: 685.00) SpeedNode 226 1.25 132 0 (cost: 73.82, dist: 31.07, risk: 10.13, smooth: 32.62) SpeedNode 231 1.06 127 0 (cost: 73.82, dist: 31.07, risk: 10.13, smooth: 32.62) Total time: 119.55 ms [ INFO] [1509354676.171312705, 860.106000000]: Creating 1 swatches [ INFO] [1509354676.439040069, 860.374000000]: Risk increase [ INFO] [1509354676.439174720, 860.374000000]: Path is bad, planning a new path [ INFO] [1509354676.439466924, 860.374000000]: OctoMap memory usage: 0.128 MB [ INFO] [1509354676.439539247, 860.374000000]: Planning a path from (42,6,7) to (31,6,3) [ INFO] [1509354676.439572982, 860.374000000]: curr_pos_: 41.93,5.92,7.60 s: 42.50,6.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 138 2 425 0 (cost: 1111.77, dist: 26.90, risk: 24.87, smooth: 595.00) SpeedNode 219 1.25 141 0 (cost: 92.43, dist: 31.78, risk: 10.98, smooth: 49.66) SpeedNode 190 1.06 138 0 (cost: 92.43, dist: 31.78, risk: 10.98, smooth: 49.66) Total time: 124.69 ms [ INFO] [1509354676.906864176, 860.840000000]: Creating 1 swatches [ INFO] [1509354676.998460622, 860.932000000]: Risk increase [ INFO] [1509354676.998498222, 860.932000000]: Path is bad, planning a new path [ INFO] [1509354676.998633794, 860.932000000]: OctoMap memory usage: 0.131 MB [ INFO] [1509354676.998655594, 860.932000000]: Planning a path from (42,7,7) to (31,6,3) [ INFO] [1509354676.998667232, 860.932000000]: curr_pos_: 41.96,6.67,7.61 s: 42.50,7.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 145 2 451 0 (cost: 1227.74, dist: 27.90, risk: 24.85, smooth: 705.00) SpeedNode 224 1.25 224 0 (cost: 100.83, dist: 32.60, risk: 11.85, smooth: 56.38) SpeedNode 210 1.06 225 0 (cost: 100.83, dist: 32.60, risk: 11.85, smooth: 56.38) Total time: 169.85 ms [ INFO] [1509354678.026921352, 861.960000000]: Creating 1 swatches [ INFO] [1509354678.613856862, 862.546000000]: Risk increase [ INFO] [1509354678.613883374, 862.546000000]: Path is bad, planning a new path [ INFO] [1509354678.614062694, 862.546000000]: OctoMap memory usage: 0.135 MB [ INFO] [1509354678.614087034, 862.546000000]: Planning a path from (43,9,7) to (31,6,3) [ INFO] [1509354678.614100389, 862.546000000]: curr_pos_: 43.05,8.86,7.66 s: 43.50,9.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 147 2 545 0 (cost: 1100.22, dist: 30.31, risk: 24.90, smooth: 705.00) SpeedNode 268 1.25 72 0 (cost: 96.12, dist: 30.98, risk: 11.46, smooth: 50.31) SpeedNode 265 1.06 86 0 (cost: 97.24, dist: 34.40, risk: 12.52, smooth: 50.31) Total time: 128.72 ms [ INFO] [1509354679.595044070, 863.526000000]: Creating 1 swatches [ INFO] [1509354680.299234910, 864.230000000]: Creating 1 swatches [ INFO] [1509354680.703038168, 864.634000000]: Risk increase [ INFO] [1509354680.703123088, 864.634000000]: Path is bad, planning a new path [ INFO] [1509354680.703285130, 864.634000000]: OctoMap memory usage: 0.139 MB [ INFO] [1509354680.703306539, 864.634000000]: Planning a path from (45,12,7) to (31,6,3) [ INFO] [1509354680.703318231, 864.634000000]: curr_pos_: 44.72,11.67,7.62 s: 45.50,12.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 134 2 644 0 (cost: 1106.14, dist: 29.56, risk: 30.42, smooth: 750.58) SpeedNode 223 1.25 87 0 (cost: 98.23, dist: 32.73, risk: 14.70, smooth: 50.80) SpeedNode 216 1.06 93 0 (cost: 98.23, dist: 32.73, risk: 14.70, smooth: 50.80) Total time: 132.30 ms [ INFO] [1509354681.066577360, 864.998000000]: Creating 1 swatches [ INFO] [1509354681.273426796, 865.204000000]: Risk increase [ INFO] [1509354681.273513523, 865.204000000]: Path is bad, planning a new path [ INFO] [1509354681.273685421, 865.204000000]: OctoMap memory usage: 0.141 MB [ INFO] [1509354681.273714002, 865.204000000]: Planning a path from (45,13,7) to (31,6,3) [ INFO] [1509354681.273729483, 865.204000000]: curr_pos_: 45.18,12.41,7.61 s: 45.50,13.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 135 2 658 0 (cost: 425.60, dist: 34.56, risk: 14.88, smooth: 325.58) SpeedNode 246 1.25 76 0 (cost: 95.90, dist: 33.73, risk: 11.37, smooth: 50.80) SpeedNode 230 1.06 76 0 (cost: 95.90, dist: 33.73, risk: 11.37, smooth: 50.80) Total time: 131.51 ms [ INFO] [1509354681.707063198, 865.638000000]: Creating 1 swatches [ INFO] [1509354681.866134574, 865.796000000]: Risk increase [ INFO] [1509354681.866172419, 865.796000000]: Path is bad, planning a new path [ INFO] [1509354681.866548901, 865.798000000]: OctoMap memory usage: 0.142 MB [ INFO] [1509354681.866662092, 865.798000000]: Planning a path from (45,13,7) to (31,6,3) [ INFO] [1509354681.866694323, 865.798000000]: curr_pos_: 45.48,13.12,7.61 s: 45.50,13.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 136 2 712 0 (cost: 1245.88, dist: 29.97, risk: 30.91, smooth: 810.00) SpeedNode 251 1.25 160 0 (cost: 88.02, dist: 33.10, risk: 14.54, smooth: 40.38) SpeedNode 219 1.06 142 0 (cost: 91.15, dist: 33.17, risk: 14.64, smooth: 43.35) Total time: 175.74 ms [ INFO] [1509354682.314564618, 866.244000000]: Creating 1 swatches [ INFO] [1509354682.514322731, 866.444000000]: Risk increase [ INFO] [1509354682.514350977, 866.444000000]: Path is bad, planning a new path [ INFO] [1509354682.514491397, 866.444000000]: OctoMap memory usage: 0.143 MB [ INFO] [1509354682.514537199, 866.444000000]: Planning a path from (45,14,7) to (31,6,3) [ INFO] [1509354682.514553131, 866.444000000]: curr_pos_: 45.69,14.01,7.60 s: 45.50,14.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 131 2 696 0 (cost: 454.77, dist: 30.14, risk: 29.63, smooth: 310.00) SpeedNode 232 1.25 81 0 (cost: 88.44, dist: 33.50, risk: 11.11, smooth: 43.83) SpeedNode 219 1.06 75 0 (cost: 88.44, dist: 33.50, risk: 11.11, smooth: 43.83) Total time: 132.98 ms [ INFO] [1509354683.051036735, 866.980000000]: Creating 1 swatches [ INFO] [1509354683.883174706, 867.812000000]: Creating 1 swatches [ INFO] [1509354684.586989992, 868.514000000]: Creating 1 swatches [ INFO] [1509354684.603442224, 868.532000000]: Risk increase [ INFO] [1509354684.603513973, 868.532000000]: Path is bad, planning a new path [ INFO] [1509354684.603731483, 868.532000000]: OctoMap memory usage: 0.147 MB [ INFO] [1509354684.603755120, 868.532000000]: Planning a path from (45,17,7) to (31,6,3) [ INFO] [1509354684.603766993, 868.532000000]: curr_pos_: 45.64,16.83,7.59 s: 45.50,17.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 131 2 744 0 (cost: 785.69, dist: 30.31, risk: 20.38, smooth: 635.00) SpeedNode 208 1.25 154 0 (cost: 108.41, dist: 35.26, risk: 16.46, smooth: 56.68) SpeedNode 197 1.06 154 0 (cost: 90.64, dist: 34.50, risk: 12.31, smooth: 43.83) Total time: 166.20 ms [ INFO] [1509354685.259121859, 869.186000000]: Creating 1 swatches [ INFO] [1509354686.123334739, 870.050000000]: Creating 1 swatches [ INFO] [1509354686.731010780, 870.656000000]: Creating 1 swatches [ INFO] [1509354687.435225086, 871.360000000]: Creating 1 swatches [ INFO] [1509354688.228139336, 872.152000000]: Risk increase [ INFO] [1509354688.228164294, 872.152000000]: Path is bad, planning a new path [ INFO] [1509354688.228359460, 872.152000000]: OctoMap memory usage: 0.157 MB [ INFO] [1509354688.228386137, 872.152000000]: Planning a path from (44,22,7) to (31,6,3) [ INFO] [1509354688.228397996, 872.152000000]: curr_pos_: 44.54,21.76,7.52 s: 44.50,22.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 163 2 166 0 (cost: 645.52, dist: 30.66, risk: 9.86, smooth: 405.00) SpeedNode 207 1.25 228 0 (cost: 92.61, dist: 37.30, risk: 12.41, smooth: 42.90) SpeedNode 182 1.06 233 0 (cost: 92.61, dist: 37.30, risk: 12.41, smooth: 42.90) Total time: 125.09 ms [ INFO] [1509354689.258732619, 873.180000000]: Creating 1 swatches [ INFO] [1509354689.547257067, 873.470000000]: Creating 1 swatches [ INFO] [1509354689.931373590, 873.852000000]: Creating 1 swatches [ INFO] [1509354690.570538589, 874.492000000]: Creating 1 swatches [ INFO] [1509354691.275245788, 875.196000000]: Creating 1 swatches [ INFO] [1509354696.319600458, 880.236000000]: Risk increase [ INFO] [1509354696.319648466, 880.236000000]: Path is bad, planning a new path [ INFO] [1509354696.319853413, 880.236000000]: OctoMap memory usage: 0.175 MB [ INFO] [1509354696.319891608, 880.236000000]: Planning a path from (38,26,7) to (31,6,3) [ INFO] [1509354696.319911729, 880.236000000]: curr_pos_: 39.15,27.15,7.52 s: 38.50,26.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 234 2 57 0 (cost: 221.00, dist: 26.90, risk: 9.10, smooth: 185.00) SpeedNode 417 1.25 86 0 (cost: 47.15, dist: 26.42, risk: 8.80, smooth: 11.92) SpeedNode 272 1.06 51 0 (cost: 46.98, dist: 26.39, risk: 8.57, smooth: 12.02) Total time: 67.98 ms [ INFO] [1509354697.326896029, 881.242000000]: Risk increase [ INFO] [1509354697.326943030, 881.242000000]: Path is bad, planning a new path [ INFO] [1509354697.327488389, 881.242000000]: OctoMap memory usage: 0.178 MB [ INFO] [1509354697.327595722, 881.242000000]: Planning a path from (38,26,7) to (31,6,3) [ INFO] [1509354697.327640766, 881.242000000]: curr_pos_: 38.57,26.53,7.54 s: 38.50,26.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 186 2 60 0 (cost: 261.23, dist: 27.49, risk: 8.75, smooth: 205.00) SpeedNode 189 1.25 515 0 (cost: 97.50, dist: 28.02, risk: 13.83, smooth: 48.18) SpeedNode 193 1.06 541 0 (cost: 66.43, dist: 28.89, risk: 11.89, smooth: 25.65) Total time: 221.91 ms [ INFO] [1509354697.972916876, 881.888000000]: Risk increase [ INFO] [1509354697.973010562, 881.888000000]: Path is bad, planning a new path [ INFO] [1509354697.973370347, 881.888000000]: OctoMap memory usage: 0.180 MB [ INFO] [1509354697.973411298, 881.888000000]: Planning a path from (37,25,7) to (31,6,3) [ INFO] [1509354697.973435078, 881.888000000]: curr_pos_: 38.00,26.13,7.54 s: 37.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 164 2 60 0 (cost: 261.11, dist: 27.49, risk: 8.62, smooth: 225.00) SpeedNode 233 1.25 140 0 (cost: 59.69, dist: 26.93, risk: 9.83, smooth: 22.92) SpeedNode 209 1.06 147 0 (cost: 59.69, dist: 26.93, risk: 9.83, smooth: 22.92) Total time: 79.47 ms [ INFO] [1509354698.483534750, 882.396000000]: Risk increase [ INFO] [1509354698.483673855, 882.396000000]: Path is bad, planning a new path [ INFO] [1509354698.484541090, 882.398000000]: OctoMap memory usage: 0.182 MB [ INFO] [1509354698.484608024, 882.398000000]: Planning a path from (37,26,7) to (31,6,3) [ INFO] [1509354698.484658448, 882.398000000]: curr_pos_: 37.56,26.08,7.55 s: 37.50,26.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 174 2 160 0 (cost: 308.62, dist: 27.90, risk: 25.73, smooth: 235.00) SpeedNode 199 1.25 511 0 (cost: 68.48, dist: 31.19, risk: 10.59, smooth: 26.70) SpeedNode 183 1.06 464 0 (cost: 66.23, dist: 29.37, risk: 10.81, smooth: 26.05) Total time: 227.55 ms [ INFO] [1509354699.181120162, 883.094000000]: Risk increase [ INFO] [1509354699.181150900, 883.094000000]: Path is bad, planning a new path [ INFO] [1509354699.181394230, 883.094000000]: OctoMap memory usage: 0.183 MB [ INFO] [1509354699.181449598, 883.094000000]: Planning a path from (36,25,7) to (31,6,3) [ INFO] [1509354699.181482102, 883.094000000]: curr_pos_: 36.82,25.82,7.55 s: 36.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 150 2 272 0 (cost: 559.74, dist: 33.90, risk: 10.84, smooth: 515.00) SpeedNode 182 1.25 206 0 (cost: 67.90, dist: 28.78, risk: 9.91, smooth: 29.21) SpeedNode 163 1.06 250 0 (cost: 67.90, dist: 28.78, risk: 9.91, smooth: 29.21) Total time: 125.72 ms [ INFO] [1509354699.738527708, 883.648000000]: Risk increase [ INFO] [1509354699.738599225, 883.648000000]: Path is bad, planning a new path [ INFO] [1509354699.739014315, 883.650000000]: OctoMap memory usage: 0.185 MB [ INFO] [1509354699.739048756, 883.650000000]: Planning a path from (35,25,7) to (31,6,3) [ INFO] [1509354699.739068028, 883.650000000]: curr_pos_: 36.30,25.83,7.53 s: 35.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 154 2 221 0 (cost: 351.39, dist: 28.31, risk: 18.07, smooth: 285.00) SpeedNode 220 1.25 255 0 (cost: 88.97, dist: 29.47, risk: 9.88, smooth: 49.61) SpeedNode 206 1.06 240 0 (cost: 88.97, dist: 29.47, risk: 9.88, smooth: 49.61) Total time: 146.33 ms [ INFO] [1509354700.823140558, 884.734000000]: Risk increase [ INFO] [1509354700.823222498, 884.734000000]: Path is bad, planning a new path [ INFO] [1509354700.823757311, 884.734000000]: OctoMap memory usage: 0.188 MB [ INFO] [1509354700.823807165, 884.734000000]: Planning a path from (34,25,7) to (31,6,3) [ INFO] [1509354700.823828659, 884.734000000]: curr_pos_: 34.91,25.70,7.53 s: 34.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 157 2 429 0 (cost: 490.83, dist: 27.90, risk: 52.93, smooth: 390.00) SpeedNode 176 1.25 236 0 (cost: 68.95, dist: 29.23, risk: 10.06, smooth: 29.66) SpeedNode 158 1.06 236 0 (cost: 68.95, dist: 29.23, risk: 10.06, smooth: 29.66) Total time: 152.65 ms [ INFO] [1509354701.433166562, 885.342000000]: Risk increase [ INFO] [1509354701.433263938, 885.342000000]: Path is bad, planning a new path [ INFO] [1509354701.433780836, 885.342000000]: OctoMap memory usage: 0.190 MB [ INFO] [1509354701.433850178, 885.342000000]: Planning a path from (33,25,7) to (31,6,3) [ INFO] [1509354701.433880027, 885.342000000]: curr_pos_: 34.05,25.56,7.53 s: 33.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 143 2 374 0 (cost: 419.04, dist: 29.14, risk: 14.90, smooth: 355.00) SpeedNode 167 1.25 192 0 (cost: 93.45, dist: 30.55, risk: 10.33, smooth: 52.57) SpeedNode 155 1.06 226 0 (cost: 71.80, dist: 29.13, risk: 10.05, smooth: 32.62) Total time: 127.62 ms [ INFO] [1509354702.040204344, 885.942000000]: Risk increase [ INFO] [1509354702.040527429, 885.948000000]: Path is bad, planning a new path [ INFO] [1509354702.040942219, 885.948000000]: OctoMap memory usage: 0.192 MB [ INFO] [1509354702.041020045, 885.948000000]: Planning a path from (32,25,7) to (31,6,3) [ INFO] [1509354702.041157693, 885.948000000]: curr_pos_: 33.23,25.39,7.53 s: 32.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 169 2 255 0 (cost: 648.31, dist: 29.56, risk: 33.76, smooth: 565.00) SpeedNode 176 1.25 214 0 (cost: 86.99, dist: 31.38, risk: 14.69, smooth: 40.93) SpeedNode 181 1.06 274 0 (cost: 93.27, dist: 30.01, risk: 10.24, smooth: 53.02) Total time: 136.92 ms [ INFO] [1509354702.641834537, 886.550000000]: Risk increase [ INFO] [1509354702.641930642, 886.550000000]: Path is bad, planning a new path [ INFO] [1509354702.642543484, 886.550000000]: OctoMap memory usage: 0.193 MB [ INFO] [1509354702.642646683, 886.550000000]: Planning a path from (31,25,7) to (31,6,3) [ INFO] [1509354702.642685890, 886.550000000]: curr_pos_: 32.36,25.33,7.53 s: 31.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 156 2 271 0 (cost: 401.91, dist: 35.97, risk: 10.94, smooth: 315.00) SpeedNode 199 1.25 177 0 (cost: 81.64, dist: 32.15, risk: 13.05, smooth: 36.43) SpeedNode 186 1.06 160 0 (cost: 97.52, dist: 30.23, risk: 10.31, smooth: 56.98) Total time: 114.28 ms [ INFO] [1509354703.189322989, 887.096000000]: Risk increase [ INFO] [1509354703.189497443, 887.096000000]: Path is bad, planning a new path [ INFO] [1509354703.190100175, 887.098000000]: OctoMap memory usage: 0.195 MB [ INFO] [1509354703.190204698, 887.098000000]: Planning a path from (30,25,7) to (31,6,3) [ INFO] [1509354703.190242766, 887.098000000]: curr_pos_: 31.46,25.34,7.53 s: 30.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 147 2 315 0 (cost: 364.59, dist: 36.38, risk: 13.21, smooth: 275.00) SpeedNode 182 1.25 232 0 (cost: 103.48, dist: 33.02, risk: 11.18, smooth: 59.28) SpeedNode 185 1.06 229 0 (cost: 103.48, dist: 33.02, risk: 11.18, smooth: 59.28) Total time: 139.09 ms [ INFO] [1509354703.793468185, 887.700000000]: Risk increase [ INFO] [1509354703.793503746, 887.700000000]: Path is bad, planning a new path [ INFO] [1509354703.793716598, 887.700000000]: OctoMap memory usage: 0.197 MB [ INFO] [1509354703.793748828, 887.700000000]: Planning a path from (29,25,7) to (31,6,3) [ INFO] [1509354703.793770571, 887.700000000]: curr_pos_: 30.57,25.34,7.54 s: 29.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 148 2 479 0 (cost: 343.28, dist: 42.80, risk: 5.48, smooth: 285.00) SpeedNode 178 1.25 231 0 (cost: 81.40, dist: 33.84, risk: 11.63, smooth: 35.93) SpeedNode 164 1.06 227 0 (cost: 81.40, dist: 33.84, risk: 11.63, smooth: 35.93) Total time: 157.38 ms [ INFO] [1509354705.391729372, 889.298000000]: Risk increase [ INFO] [1509354705.391760178, 889.298000000]: Path is bad, planning a new path [ INFO] [1509354705.392033224, 889.298000000]: OctoMap memory usage: 0.201 MB [ INFO] [1509354705.392071325, 889.298000000]: Planning a path from (27,25,7) to (31,6,3) [ INFO] [1509354705.392100991, 889.298000000]: curr_pos_: 28.13,25.17,7.53 s: 27.50,25.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 146 2 543 0 (cost: 240.45, dist: 36.80, risk: 8.65, smooth: 195.00) SpeedNode 220 1.25 169 0 (cost: 101.27, dist: 32.20, risk: 10.93, smooth: 58.14) SpeedNode 194 1.06 184 0 (cost: 78.74, dist: 32.15, risk: 12.31, smooth: 34.28) Total time: 159.20 ms [ INFO] [1509354706.034477796, 889.940000000]: Risk increase [ INFO] [1509354706.034514838, 889.940000000]: Path is bad, planning a new path [ INFO] [1509354706.034713418, 889.940000000]: OctoMap memory usage: 0.202 MB [ INFO] [1509354706.034785668, 889.940000000]: Planning a path from (26,24,7) to (31,6,3) [ INFO] [1509354706.034810723, 889.940000000]: curr_pos_: 27.12,25.00,7.54 s: 26.50,24.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 157 2 571 0 (cost: 207.25, dist: 36.80, risk: 15.45, smooth: 135.00) SpeedNode 270 1.25 93 0 (cost: 75.00, dist: 32.74, risk: 10.73, smooth: 31.54) SpeedNode 262 1.06 92 0 (cost: 75.00, dist: 32.74, risk: 10.73, smooth: 31.54) Total time: 148.39 ms [ INFO] [1509354711.148616938, 895.048000000]: Risk increase [ INFO] [1509354711.148719734, 895.048000000]: Path is bad, planning a new path [ INFO] [1509354711.149259130, 895.048000000]: OctoMap memory usage: 0.215 MB [ INFO] [1509354711.149331956, 895.048000000]: Planning a path from (20,20,7) to (31,6,3) [ INFO] [1509354711.149354851, 895.048000000]: curr_pos_: 20.92,20.66,7.53 s: 20.50,20.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 164 2 149 0 (cost: 280.35, dist: 34.90, risk: 10.45, smooth: 195.00) SpeedNode 249 1.25 854 0 (cost: 114.72, dist: 30.02, risk: 23.72, smooth: 60.98) SpeedNode 247 1.06 997 0 ===== Half-way path ====== [ INFO] [1509354711.629645412, 895.528000000]: ========== Set goal : (17,12,5) ========== [ INFO] [1509354711.638224918, 895.536000000]: OctoMap memory usage: 0.215 MB [ INFO] [1509354711.638290500, 895.536000000]: Planning a path from (20,20,7) to (17,12,5) [ INFO] [1509354711.638314412, 895.536000000]: curr_pos_: 20.92,20.66,7.53 s: 20.50,20.50,7.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 566 2 11 0 (cost: 277.73, dist: 10.24, risk: 2.49, smooth: 225.00) SpeedNode 272 1.25 12 0 (cost: 30.62, dist: 9.71, risk: 2.23, smooth: 15.33) SpeedNode 241 1.06 13 0 (cost: 30.62, dist: 9.71, risk: 2.23, smooth: 15.33) Total time: 15.59 ms Total time: 525.77 ms [ INFO] [1509354716.599992587, 900.486000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354716.600686734, 900.490000000]: OctoMap memory usage: 0.221 MB [ INFO] [1509354716.600851005, 900.490000000]: Planning a path from (17,12,6) to (31,6,3) [ INFO] [1509354716.600915709, 900.490000000]: curr_pos_: 17.13,13.59,6.98 s: 17.50,12.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 148 2 2000 0 No path found, search in 2D 789.80 2.00 5 20.13 66.00 2.00 8 10.92 41.15 2.00 540 53.04 ===== Half-way path ====== [ INFO] [1509354716.927206682, 900.806000000]: ========== Set goal : (22,0,10) ========== [ INFO] [1509354716.928619139, 900.806000000]: OctoMap memory usage: 0.221 MB [ INFO] [1509354716.928740860, 900.806000000]: Planning a path from (17,12,6) to (22,0,10) [ INFO] [1509354716.928799555, 900.806000000]: curr_pos_: 17.13,13.59,6.98 s: 17.50,12.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 314 2 18 0 (cost: 225.20, dist: 14.66, risk: 0.54, smooth: 145.00) SpeedNode 232 1.25 24 0 (cost: 36.24, dist: 14.05, risk: 1.47, smooth: 15.72) SpeedNode 243 1.06 25 0 (cost: 36.24, dist: 14.05, risk: 1.47, smooth: 15.72) Total time: 21.76 ms Total time: 363.60 ms [ INFO] [1509354716.948646415, 900.826000000]: Reached current goal (22,0,10), 1 goals left [ INFO] [1509354716.948712504, 900.826000000]: Actual travel distance: 111.46 Actual energy usage: 163.99 [ INFO] [1509354718.414961692, 902.290000000]: Risk increase [ INFO] [1509354718.415009911, 902.290000000]: Path is bad, planning a new path [ INFO] [1509354718.415056867, 902.290000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354718.415441537, 902.290000000]: OctoMap memory usage: 0.220 MB [ INFO] [1509354718.415535010, 902.290000000]: Planning a path from (16,10,6) to (31,6,3) [ INFO] [1509354718.415569474, 902.290000000]: curr_pos_: 16.95,11.31,6.75 s: 16.50,10.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 147 2 2096 0 (cost: 591.00, dist: 49.56, risk: 25.27, smooth: 465.58) SpeedNode 242 1.25 2904 0 ===== Half-way path ====== [ INFO] [1509354719.417618498, 903.288000000]: ========== Set goal : (30,-2,6) ========== [ INFO] [1509354719.419770844, 903.292000000]: OctoMap memory usage: 0.220 MB [ INFO] [1509354719.419816554, 903.292000000]: Planning a path from (16,10,6) to (30,-2,6) [ INFO] [1509354719.419837417, 903.292000000]: curr_pos_: 16.95,11.31,6.75 s: 16.50,10.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 264 2 34 0 (cost: 220.92, dist: 19.07, risk: 0.68, smooth: 150.58) SpeedNode 375 1.25 10 0 (cost: 39.00, dist: 18.68, risk: 1.32, smooth: 18.35) SpeedNode 292 1.06 10 0 (cost: 39.00, dist: 18.68, risk: 1.32, smooth: 18.35) Total time: 19.22 ms Total time: 1065.67 ms [ INFO] [1509354721.376007479, 905.236000000]: Risk increase [ INFO] [1509354721.376057698, 905.236000000]: Path is bad, planning a new path [ INFO] [1509354721.376088929, 905.236000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354721.376626487, 905.240000000]: OctoMap memory usage: 0.219 MB [ INFO] [1509354721.376778439, 905.240000000]: Planning a path from (16,6,6) to (31,6,3) [ INFO] [1509354721.376874701, 905.240000000]: curr_pos_: 16.67,6.91,6.71 s: 16.50,6.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 187 2 1810 0 (cost: 597.00, dist: 46.97, risk: 25.03, smooth: 485.00) SpeedNode 242 1.25 3190 0 ===== Half-way path ====== [ INFO] [1509354722.473962294, 906.330000000]: ========== Set goal : (32,-2,6) ========== [ INFO] [1509354722.478368729, 906.336000000]: OctoMap memory usage: 0.219 MB [ INFO] [1509354722.478564321, 906.336000000]: Planning a path from (16,6,6) to (32,-2,6) [ INFO] [1509354722.478626510, 906.336000000]: curr_pos_: 16.67,6.91,6.71 s: 16.50,6.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 214 2 27 0 (cost: 178.24, dist: 17.07, risk: 1.17, smooth: 120.00) SpeedNode 301 1.25 16 0 (cost: 47.00, dist: 22.28, risk: 2.12, smooth: 22.60) SpeedNode 287 1.06 14 0 (cost: 39.89, dist: 18.12, risk: 1.11, smooth: 20.65) Total time: 18.93 ms Total time: 1168.74 ms [ INFO] [1509354733.414753034, 917.234000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354733.415381402, 917.236000000]: OctoMap memory usage: 0.219 MB [ INFO] [1509354733.415517496, 917.236000000]: Planning a path from (23,-2,6) to (31,6,3) [ INFO] [1509354733.415550693, 917.236000000]: curr_pos_: 22.84,-1.74,6.43 s: 23.50,-1.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 150 2 903 0 (cost: 343.04, dist: 34.07, risk: 23.97, smooth: 285.00) SpeedNode 199 1.25 2174 0 (cost: 85.25, dist: 34.12, risk: 22.45, smooth: 28.68) ===== Half-way path ====== [ INFO] [1509354733.982032939, 917.802000000]: ========== Set goal : (36,-2,5) ========== [ INFO] [1509354733.983598526, 917.802000000]: OctoMap memory usage: 0.219 MB [ INFO] [1509354733.983645173, 917.804000000]: Planning a path from (23,-2,6) to (36,-2,5) [ INFO] [1509354733.983664319, 917.804000000]: curr_pos_: 22.84,-1.74,6.43 s: 23.50,-1.50,6.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 206 2 12 0 (cost: 12.10, dist: 12.00, risk: 0.10, smooth: 0.00) SpeedNode 213 1.25 6 0 (cost: 24.82, dist: 14.00, risk: 0.82, smooth: 10.00) SpeedNode 228 1.06 6 0 (cost: 24.82, dist: 14.00, risk: 0.82, smooth: 10.00) Total time: 7.46 ms Total time: 600.79 ms [ INFO] [1509354733.990468284, 917.810000000]: Reached current goal (36,-2,5), 1 goals left [ INFO] [1509354733.990536653, 917.810000000]: Actual travel distance: 132.43 Actual energy usage: 185.20 [ INFO] [1509354739.817728992, 923.622000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354739.818202515, 923.622000000]: OctoMap memory usage: 0.224 MB [ INFO] [1509354739.818266300, 923.622000000]: Planning a path from (32,-2,5) to (31,6,3) [ INFO] [1509354739.818290669, 923.622000000]: curr_pos_: 31.94,-1.79,5.39 s: 32.50,-1.50,5.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 144 2 5000 0 No path found, search in 2D 657.17 2.00 6 25.21 218.12 2.00 8 10.92 46.67 2.00 990 61.77 ===== Half-way path ====== [ INFO] [1509354740.582902679, 924.386000000]: ========== Set goal : (37,2,10) ========== [ INFO] [1509354740.584805618, 924.386000000]: OctoMap memory usage: 0.224 MB [ INFO] [1509354740.584852785, 924.386000000]: Planning a path from (32,-2,5) to (37,2,10) [ INFO] [1509354740.584877101, 924.386000000]: curr_pos_: 31.94,-1.79,5.39 s: 32.50,-1.50,5.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 288 2 31 0 (cost: 102.80, dist: 26.41, risk: 21.38, smooth: 50.00) SpeedNode 188 1.25 48 0 (cost: 57.32, dist: 27.00, risk: 20.32, smooth: 10.00) SpeedNode 158 1.06 48 0 (cost: 57.32, dist: 27.00, risk: 20.32, smooth: 10.00) Total time: 30.81 ms Total time: 827.98 ms [ INFO] [1509354740.613637028, 924.416000000]: Reached current goal (37,2,10), 1 goals left [ INFO] [1509354740.613690996, 924.416000000]: Actual travel distance: 141.55 Actual energy usage: 194.42 [ INFO] [1509354743.090775310, 926.890000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354743.091143634, 926.890000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354743.091194033, 926.890000000]: Planning a path from (37,-2,8) to (31,6,3) [ INFO] [1509354743.091207583, 926.890000000]: curr_pos_: 36.56,-1.64,7.58 s: 37.50,-1.50,8.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 146 2 5000 0 No path found, search in 2D 213.33 2.00 3 10.03 411.75 2.00 8 10.92 42.92 2.00 2534 91.63 ===== Half-way path ====== [ INFO] [1509354743.932423812, 927.732000000]: ========== Set goal : (33,5,10) ========== [ INFO] [1509354743.933637558, 927.732000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354743.933695755, 927.732000000]: Planning a path from (37,-2,8) to (33,5,10) [ INFO] [1509354743.933722154, 927.732000000]: curr_pos_: 36.56,-1.64,7.58 s: 37.50,-1.50,8.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 133 2 5000 0 No path found, search in 2D 76.00 2.00 3 10.03 1.00 2.00 1 0.00 39.21 2.00 2248 89.18 ===== Half-way path ====== [ INFO] [1509354744.685590874, 928.484000000]: ========== Set goal : (38,0,10) ========== [ INFO] [1509354744.687246086, 928.486000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354744.687290980, 928.486000000]: Planning a path from (37,-2,8) to (38,0,10) [ INFO] [1509354744.687313194, 928.486000000]: curr_pos_: 36.56,-1.64,7.58 s: 37.50,-1.50,8.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 248 2 3 0 (cost: 262.54, dist: 7.41, risk: 0.13, smooth: 190.00) SpeedNode 769 1.25 7 0 (cost: 60.89, dist: 15.41, risk: 0.48, smooth: 45.00) SpeedNode 208 1.06 7 0 (cost: 60.89, dist: 15.41, risk: 0.48, smooth: 45.00) Total time: 12.10 ms Total time: 794.20 ms Total time: 1669.68 ms [ INFO] [1509354744.698559979, 928.496000000]: Reached current goal (38,0,10), 2 goals left [ INFO] [1509354744.698602061, 928.496000000]: Actual travel distance: 147.03 Actual energy usage: 210.74 [ INFO] [1509354744.852162732, 928.650000000]: ========== Set goal : (33,5,10) ========== [ INFO] [1509354744.852473805, 928.650000000]: OctoMap memory usage: 0.232 MB [ INFO] [1509354744.852568176, 928.650000000]: Planning a path from (38,-1,10) to (33,5,10) [ INFO] [1509354744.852605124, 928.650000000]: curr_pos_: 37.93,-0.83,9.43 s: 38.50,-0.50,10.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 139 2 5000 0 No path found, search in 2D 6.00 2.00 1 0.00 2.00 2.00 1 0.00 [ INFO] [1509354745.744310348, 929.542000000]: Failed to find a path, risk of t: 0.00 [ INFO] [1509354745.762762040, 929.560000000]: Failed to find a path Total time: 943.85 ms [ INFO] [1509354745.762803647, 929.560000000]: Reached current goal (33,5,10), 1 goals left [ INFO] [1509354745.762849507, 929.560000000]: Actual travel distance: 147.03 Actual energy usage: 210.74 [ INFO] [1509354745.802194044, 929.600000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354745.804074687, 929.600000000]: OctoMap memory usage: 0.232 MB [ INFO] [1509354745.804419124, 929.602000000]: Planning a path from (38,-2,10) to (31,6,3) [ INFO] [1509354745.804470728, 929.602000000]: curr_pos_: 38.64,-1.08,9.43 s: 38.50,-1.50,10.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 131 2 5000 0 No path found, search in 2D 20.00 2.00 1 0.00 557.00 2.00 8 10.92 37.96 2.00 4794 121.93 ===== Half-way path ====== [ INFO] [1509354746.648330815, 930.446000000]: ========== Set goal : (24,-1,10) ========== [ INFO] [1509354746.650481523, 930.448000000]: OctoMap memory usage: 0.232 MB [ INFO] [1509354746.650529714, 930.448000000]: Planning a path from (38,-2,10) to (24,-1,10) [ INFO] [1509354746.650547422, 930.448000000]: curr_pos_: 38.64,-1.08,9.43 s: 38.50,-1.50,10.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 339 2 14 0 (cost: 24.20, dist: 14.00, risk: 0.20, smooth: 5.00) SpeedNode 15 1.25 1 0 ===== Half-way path ====== [ INFO] [1509354746.658285273, 930.456000000]: ========== Set goal : (32,-2,9) ========== [ INFO] [1509354746.658589585, 930.456000000]: OctoMap memory usage: 0.232 MB [ INFO] [1509354746.658621735, 930.456000000]: Planning a path from (38,-2,10) to (32,-2,9) [ INFO] [1509354746.658640057, 930.456000000]: curr_pos_: 38.64,-1.08,9.43 s: 38.50,-1.50,10.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 132 2 6 0 (cost: 16.10, dist: 6.00, risk: 0.10, smooth: 5.00) SpeedNode 21 1.25 1 0 Total time: 1.90 ms Total time: 10.74 ms Total time: 886.27 ms [ INFO] [1509354751.046361902, 934.838000000]: ========== Set goal : (24,-1,10) ========== [ INFO] [1509354751.047010090, 934.840000000]: OctoMap memory usage: 0.232 MB [ INFO] [1509354751.047167527, 934.840000000]: Planning a path from (35,-2,9) to (24,-1,10) [ INFO] [1509354751.047208503, 934.840000000]: curr_pos_: 36.25,-1.49,9.28 s: 35.50,-1.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 186 2 10 0 (cost: 10.12, dist: 10.00, risk: 0.12, smooth: 0.00) SpeedNode 231 1.25 17 0 (cost: 36.87, dist: 11.16, risk: 22.35, smooth: 3.36) SpeedNode 243 1.06 17 0 (cost: 36.87, dist: 11.16, risk: 22.35, smooth: 3.36) Total time: 12.45 ms [ INFO] [1509354751.058131419, 934.850000000]: Reached current goal (24,-1,10), 1 goals left [ INFO] [1509354751.058207300, 934.850000000]: Actual travel distance: 153.41 Actual energy usage: 227.44 [ INFO] [1509354755.835771075, 939.624000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354755.836019902, 939.624000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354755.836062500, 939.624000000]: Planning a path from (29,-2,9) to (31,6,3) [ INFO] [1509354755.836077633, 939.624000000]: curr_pos_: 30.16,-1.52,9.20 s: 29.50,-1.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 145 2 5000 0 No path found, search in 2D 119.50 2.00 2 5.01 646.88 2.00 8 10.92 45.56 2.00 1779 80.91 ===== Half-way path ====== [ INFO] [1509354756.652891726, 940.432000000]: ========== Set goal : (31,5,10) ========== [ INFO] [1509354756.655650328, 940.436000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354756.655789378, 940.436000000]: Planning a path from (29,-2,9) to (31,5,10) [ INFO] [1509354756.655819228, 940.436000000]: curr_pos_: 30.16,-1.52,9.20 s: 29.50,-1.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 144 2 5000 0 No path found, search in 2D 191.50 2.00 2 5.01 3.00 2.00 1 0.00 44.17 2.00 1685 79.89 ===== Half-way path ====== [ INFO] [1509354757.451604843, 941.230000000]: ========== Set goal : (30,1,10) ========== [ INFO] [1509354757.454142494, 941.232000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354757.454201099, 941.232000000]: Planning a path from (29,-2,9) to (30,1,10) [ INFO] [1509354757.454233223, 941.232000000]: curr_pos_: 30.16,-1.52,9.20 s: 29.50,-1.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 140 2 5000 0 No path found, search in 2D 349.50 2.00 2 5.01 2.00 2.00 1 0.00 42.89 2.00 1190 72.97 Total time: 785.41 ms Total time: 1616.63 ms Total time: 2468.34 ms [ INFO] [1509354758.208804238, 941.986000000]: Reached current goal (30,1,10), 2 goals left [ INFO] [1509354758.208853151, 941.986000000]: Actual travel distance: 159.50 Actual energy usage: 234.09 [ INFO] [1509354758.416495323, 942.194000000]: Risk increase [ INFO] [1509354758.416735356, 942.194000000]: Path is bad, planning a new path [ INFO] [1509354758.416835832, 942.194000000]: ========== Set goal : (31,5,10) ========== [ INFO] [1509354758.418816063, 942.196000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354758.418975928, 942.196000000]: Planning a path from (28,-1,10) to (31,5,10) [ INFO] [1509354758.419035205, 942.196000000]: curr_pos_: 28.40,-0.92,10.10 s: 28.50,-0.50,10.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 149 2 5000 0 No path found, search in 2D 6.00 2.00 1 0.00 2.00 2.00 1 0.00 44.43 2.00 2502 94.96 ===== Half-way path ====== [ INFO] [1509354759.266476549, 943.040000000]: ========== Set goal : (33,0,10) ========== [ INFO] [1509354759.267683078, 943.042000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354759.267722088, 943.042000000]: Planning a path from (28,-1,10) to (33,0,10) [ INFO] [1509354759.267744394, 943.042000000]: curr_pos_: 28.40,-0.92,10.10 s: 28.50,-0.50,10.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 110 2 6 0 (cost: 137.02, dist: 6.41, risk: 25.61, smooth: 105.00) SpeedNode 13 1.25 1 0 Total time: 6.31 ms Total time: 892.48 ms [ INFO] [1509354762.122406219, 945.894000000]: ========== Set goal : (31,5,10) ========== [ INFO] [1509354762.122692764, 945.894000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354762.122746192, 945.894000000]: Planning a path from (31,0,9) to (31,5,10) [ INFO] [1509354762.122765291, 945.894000000]: curr_pos_: 31.07,0.07,9.39 s: 31.50,0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 144 2 5000 0 No path found, search in 2D 61.70 2.00 760 49.70 2.00 2.00 1 0.00 [ INFO] [1509354763.091357984, 946.860000000]: Failed to find a path, risk of t: 0.00 [ INFO] [1509354763.111834870, 946.880000000]: Failed to find a path Total time: 1031.62 ms [ INFO] [1509354763.111870844, 946.880000000]: Reached current goal (31,5,10), 1 goals left [ INFO] [1509354763.111907849, 946.880000000]: Actual travel distance: 165.51 Actual energy usage: 245.02 [ INFO] [1509354763.152188195, 946.920000000]: ========== Set goal : (31,6,3) ========== [ INFO] [1509354763.154190126, 946.924000000]: OctoMap memory usage: 0.231 MB [ INFO] [1509354763.154260479, 946.924000000]: Planning a path from (32,0,9) to (31,6,3) [ INFO] [1509354763.154282722, 946.924000000]: curr_pos_: 32.26,0.28,9.37 s: 32.50,0.50,9.50 Search iter_time overest num_iter path_cost NodeWithoutSmooth 142 2 5000 0 No path found, search in 2D 60.43 2.00 779 49.70 162.25 2.00 8 10.92 [ INFO] [1509354764.092754834, 947.860000000]: Failed to find a path, risk of t: 0.00 [ INFO] [1509354764.110827861, 947.878000000]: Failed to find a path Total time: 996.52 ms [ INFO] [1509354764.141007121, 947.908000000]: STOP [ INFO] [1509354764.206338996, 947.974000000]: Received empty path ERROR [mavlink] [timesync] Hard setting offset. [ WARN] [1509354856.916919793, 1040.566000000]: TM: Clock skew detected (-0.010907426 s). Hard syncing clocks. ERROR [mavlink] [timesync] Hard setting offset. ERROR [mavlink] [timesync] Hard setting offset. ERROR [mavlink] [timesync] Hard setting offset. [ WARN] [1509354871.431250318, 1055.064000000]: TM: Clock skew detected (-0.011349102 s). Hard syncing clocks.