search

Lars-Ki / px4_rrt_avoidance

A UAV collision avoidance package for px4 applications
MIT License
9 stars 7 forks source link

map was nut fully updated yet #5

Closed boonflies closed 1 year ago

boonflies commented 2 years ago

When i try to run, master_node.py, i get the info that the 'map was not fully updated' and then the error,
return drone_1.position[5] IndexError: list index out of range.

I also find that the mavros topics are not published.

Lars-Ki commented 2 years ago

Hi, boonflies, thank you for your bug report.

May I ask: 1) Do you have mavros installed? 2) Do you first run roslaunch px4_rrt_avoidance iris_depth.launch and then rosrun px4_rrt_avoidance Master_node.py (does an other error pop up in this process?)

boonflies commented 2 years ago

Yes,i have installed mavros.

Yes, I first run 'roslaunch px4_rrt_avoidance iris_depth.launch' with gazebo gui and i get the following printed in terminal: boon@boon-Vostro-3445:~/catkin_ws/src/px4_rrt_avoidance/launch$ roslaunch px4_rrt_avoidance iris_depth_wo_rviz.launch ... logging to /home/boon/.ros/log/540ec18e-51e5-11ec-8783-c03896420cbf/roslaunch-boon-Vostro-3445-5525.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

WARN: unrecognized 'param' tag in tag started roslaunch server http://boon-Vostro-3445:45801/

SUMMARY

CLEAR PARAMETERS

PARAMETERS

NODES / camera_static_tf (tf/static_transform_publisher) companion_checker (px4_rrt_avoidance/companion_computer.py) drone_in_map (px4_rrt_avoidance/drone_in_local_map.py) dynamic_parmater (px4_rrt_avoidance/dynamic_param_reconfigure.py) gazebo (gazebo_ros/gzserver) local_map_publisher (px4_rrt_avoidance/local_map_publisher.py) local_planner (px4_rrt_avoidance/local_planner.py) mavros (mavros/mavros_node) octomap_server (octomap_server/octomap_server_node) px4_rrt_avoidance_tf (px4_rrt_avoidance/FC_pose_tf_publisher.py) rviz (rviz/rviz) sitl_0 (px4/px4)

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

setting /run_id to 540ec18e-51e5-11ec-8783-c03896420cbf process[rosout-1]: started with pid [5564] started core service [/rosout] process[gazebo-2]: started with pid [5568] process[mavros-3]: started with pid [5577] [ INFO] [1638281993.860180538]: FCU URL: udp://:14540@localhost:14557 [ INFO] [1638281993.873685368]: udp0: Bind address: 0.0.0.0:14540 [ INFO] [1638281993.874193214]: udp0: Remote address: 127.0.0.1:14557 [ INFO] [1638281993.874951537]: GCS bridge disabled [ INFO] [1638281994.078696017]: Plugin 3dr_radio loaded [ INFO] [1638281994.082760645]: Plugin 3dr_radio initialized [ INFO] [1638281994.083094199]: Plugin actuator_control loaded [ INFO] [1638281994.091912206]: Plugin actuator_control initialized [ INFO] [1638281994.221453167]: Plugin adsb loaded [ INFO] [1638281994.230655441]: Plugin adsb initialized [ INFO] [1638281994.231217168]: Plugin altitude loaded [ INFO] [1638281994.234340075]: Plugin altitude initialized [ INFO] [1638281994.234831347]: Plugin cam_imu_sync loaded [ INFO] [1638281994.236930277]: Plugin cam_imu_sync initialized [ INFO] [1638281994.237526447]: Plugin command loaded [ INFO] [1638281994.258716392]: Plugin command initialized [ INFO] [1638281994.259186488]: Plugin companion_process_status loaded [ INFO] [1638281994.265061417]: Plugin companion_process_status initialized [ INFO] [1638281994.265555199]: Plugin debug_value loaded [ INFO] [1638281994.277117001]: Plugin debug_value initialized [ INFO] [1638281994.277235171]: Plugin distance_sensor blacklisted [ INFO] [1638281994.277727222]: Plugin esc_status loaded [ INFO] [1638281994.281206578]: Plugin esc_status initialized [ INFO] [1638281994.281737628]: Plugin esc_telemetry loaded [ INFO] [1638281994.285088198]: Plugin esc_telemetry initialized [ INFO] [1638281994.285768511]: Plugin fake_gps loaded [ INFO] [1638281994.322696571]: Plugin fake_gps initialized [ INFO] [1638281994.323330395]: Plugin ftp loaded [ INFO] [1638281994.350670396]: Plugin ftp initialized [ INFO] [1638281994.351288665]: Plugin geofence loaded [ INFO] [1638281994.360502073]: Plugin geofence initialized [ INFO] [1638281994.361048624]: Plugin global_position loaded [ INFO] [1638281994.399903416]: Plugin global_position initialized [ INFO] [1638281994.400461150]: Plugin gps_input loaded [ INFO] [1638281994.407885466]: Plugin gps_input initialized [ INFO] [1638281994.408395805]: Plugin gps_rtk loaded [ INFO] [1638281994.417569594]: Plugin gps_rtk initialized [ INFO] [1638281994.418308865]: Plugin gps_status loaded [ INFO] [1638281994.424295437]: Plugin gps_status initialized [ INFO] [1638281994.424829788]: Plugin hil loaded [ INFO] [1638281994.456162299]: Plugin hil initialized [ INFO] [1638281994.456718768]: Plugin home_position loaded [ INFO] [1638281994.465636160]: Plugin home_position initialized [ INFO] [1638281994.466405957]: Plugin imu loaded [ INFO] [1638281994.486598671]: Plugin imu initialized [ INFO] [1638281994.487129847]: Plugin landing_target loaded [ INFO] [1638281994.520095161]: Plugin landing_target initialized [ INFO] [1638281994.520665699]: Plugin local_position loaded [ INFO] [1638281994.539210538]: Plugin local_position initialized [ INFO] [1638281994.539689928]: Plugin log_transfer loaded [ INFO] [1638281994.548881912]: Plugin log_transfer initialized [ INFO] [1638281994.549432434]: Plugin mag_calibration_status loaded [ INFO] [1638281994.553738177]: Plugin mag_calibration_status initialized [ INFO] [1638281994.554336053]: Plugin manual_control loaded [ INFO] [1638281994.562981370]: Plugin manual_control initialized [ INFO] [1638281994.563496649]: Plugin mocap_pose_estimate loaded [ INFO] [1638281994.573649093]: Plugin mocap_pose_estimate initialized [ INFO] [1638281994.574179925]: Plugin mount_control loaded [ INFO] [1638281994.585662748]: Plugin mount_control initialized [ INFO] [1638281994.586199631]: Plugin nav_controller_output loaded [ INFO] [1638281994.588217543]: Plugin nav_controller_output initialized [ INFO] [1638281994.588761689]: Plugin obstacle_distance loaded [ INFO] [1638281994.596385940]: Plugin obstacle_distance initialized [ INFO] [1638281994.596968264]: Plugin odom loaded [ INFO] [1638281994.608648196]: Plugin odom initialized [ INFO] [1638281994.609163376]: Plugin onboard_computer_status loaded [ INFO] [1638281994.615410482]: Plugin onboard_computer_status initialized [ INFO] [1638281994.616169575]: Plugin param loaded [ INFO] [1638281994.626691541]: Plugin param initialized [ INFO] [1638281994.627229657]: Plugin play_tune loaded [ INFO] [1638281994.634203336]: Plugin play_tune initialized [ INFO] [1638281994.634774851]: Plugin px4flow loaded [ INFO] [1638281994.652779974]: Plugin px4flow initialized [ INFO] [1638281994.653466704]: Plugin rallypoint loaded [ INFO] [1638281994.663737913]: Plugin rallypoint initialized [ INFO] [1638281994.663933220]: Plugin rangefinder blacklisted [ INFO] [1638281994.664590140]: Plugin rc_io loaded [ INFO] [1638281994.674298108]: Plugin rc_io initialized [ INFO] [1638281994.674447512]: Plugin safety_area blacklisted [ INFO] [1638281994.674954798]: Plugin setpoint_accel loaded [ INFO] [1638281994.681710546]: Plugin setpoint_accel initialized [ INFO] [1638281994.682517277]: Plugin setpoint_attitude loaded [ INFO] [1638281994.710225499]: Plugin setpoint_attitude initialized [ INFO] [1638281994.710951374]: Plugin setpoint_position loaded [ INFO] [1638281994.750672429]: Plugin setpoint_position initialized [ INFO] [1638281994.751270581]: Plugin setpoint_raw loaded [ INFO] [1638281994.774264066]: Plugin setpoint_raw initialized [ INFO] [1638281994.774837513]: Plugin setpoint_trajectory loaded [ INFO] [1638281994.788330565]: Plugin setpoint_trajectory initialized [ INFO] [1638281994.788931038]: Plugin setpoint_velocity loaded [ INFO] [1638281994.804707148]: Plugin setpoint_velocity initialized [ INFO] [1638281994.805803422]: Plugin sys_status loaded [ INFO] [1638281994.836194937]: Plugin sys_status initialized [ INFO] [1638281994.836849236]: Plugin sys_time loaded [ INFO] [1638281994.848789278]: TM: Timesync mode: MAVLINK [ INFO] [1638281994.850046864]: TM: Not publishing sim time [ INFO] [1638281994.854189035]: Plugin sys_time initialized [ INFO] [1638281994.854826979]: Plugin trajectory loaded [ INFO] [1638281994.868744973]: Plugin trajectory initialized [ INFO] [1638281994.869248339]: Plugin tunnel loaded [ INFO] [1638281994.877092011]: Plugin tunnel initialized [ INFO] [1638281994.877631895]: Plugin vfr_hud loaded [ INFO] [1638281994.879913534]: Plugin vfr_hud initialized [ INFO] [1638281994.880084424]: Plugin vibration blacklisted [ INFO] [1638281994.880556204]: Plugin vision_pose_estimate loaded [ INFO] [1638281994.898337099]: Plugin vision_pose_estimate initialized [ INFO] [1638281994.899042336]: Plugin vision_speed_estimate loaded [ INFO] [1638281994.907705954]: Plugin vision_speed_estimate initialized [ INFO] [1638281994.908321179]: Plugin waypoint loaded [ INFO] [1638281994.923205033]: Plugin waypoint initialized [ INFO] [1638281994.923369910]: Plugin wheel_odometry blacklisted [ INFO] [1638281994.924158075]: Plugin wind_estimation loaded [ INFO] [1638281994.926589826]: Plugin wind_estimation initialized [ INFO] [1638281994.926748217]: Autostarting mavlink via USB on PX4 [ INFO] [1638281994.927052327]: Built-in SIMD instructions: SSE, SSE2 [ INFO] [1638281994.927288292]: Built-in MAVLink package version: 2021.3.3 [ INFO] [1638281994.927384251]: Known MAVLink dialects: common ardupilotmega ASLUAV all autoquad icarous matrixpilot paparazzi standard uAvionix ualberta [ INFO] [1638281994.927464728]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [ INFO] [1638282019.992227841]: Finished loading Gazebo ROS API Plugin. [ INFO] [1638282019.997104365]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting... process[sitl_0-4]: started with pid [5663] process[px4_rrt_avoidance_tf-5]: started with pid [5664] process[dynamic_parmater-6]: started with pid [5665] INFO [px4] Creating symlink /home/boon/src/PX4-Autopilot/build/px4_sitl_default/etc -> /home/boon/.ros/etc process[camera_static_tf-7]: started with pid [5666]

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

px4 starting.

INFO [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0 Info: found model autostart file as SYS_AUTOSTART=10016 process[octomap_server-8]: started with pid [5685] INFO [param] selected parameter default file eeprom/parameters_10016 INFO [parameters] BSON document size 423 bytes, decoded 423 bytes [param] Loaded: eeprom/parameters_10016 process[rviz-9]: started with pid [5765] process[drone_in_map-10]: started with pid [5766] INFO [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes process[local_planner-11]: started with pid [5767] process[local_map_publisher-12]: started with pid [5769] process[companion_checker-13]: started with pid [5770] PX4 SIM HOST: localhost INFO [simulator] Waiting for simulator to accept connection on TCP port 4560 INFO: cannot create a symlink to latest log directory: [Errno 2] No such file or directory: '/home/boon/.ros/log/latest' [ INFO] [1638282033.855258581]: waitForService: Service [/gazebo/set_physics_properties] is now available. [ INFO] [1638282034.018915311]: Physics dynamic reconfigure ready. INFO: cannot create a symlink to latest log directory: [Errno 17] File exists [ WARN] [1638282036.558534467]: Nothing to publish, octree is empty INFO [simulator] Simulator connected on TCP port 4560. INFO [commander] LED: open /dev/led0 failed (22) INFO [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0 INFO [ekf2] starting instance 0, IMU:0 (1310988), MAG:0 (197388) INFO [ekf2] starting instance 1, IMU:1 (1310996), MAG:0 (197388) INFO [ekf2] starting instance 2, IMU:2 (1311004), MAG:0 (197388) INFO [ekf2] starting instance 3, IMU:0 (1310988), MAG:1 (197644) INFO [ekf2] starting instance 4, IMU:1 (1310996), MAG:1 (197644) INFO [ekf2] starting instance 5, IMU:2 (1311004), MAG:1 (197644) INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550 INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540 INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030 INFO [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280 INFO [logger] logger started (mode=all) [ INFO] [1638282039.616799169, 0.880000000]: udp0: Remote address: 127.0.0.1:14580 [ INFO] [1638282039.618340391, 0.884000000]: IMU: High resolution IMU detected! INFO [logger] Start file log (type: full) INFO [logger] [logger] ./log/2021-11-30/14_20_39.ulg INFO [logger] Opened full log file: ./log/2021-11-30/14_2039.ulg INFO [mavlink] MAVLink only on localhost (set param MAV{i}BROADCAST = 1 to enable network) INFO [mavlink] MAVLink only on localhost (set param MAV{i}_BROADCAST = 1 to enable network) INFO [px4] Startup script returned successfully pxh> INFO [mavlink] partner IP: 127.0.0.1 INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero [ INFO] [1638282040.231086275]: rviz version 1.13.21 [ INFO] [1638282040.231478502]: compiled against Qt version 5.9.5 [ INFO] [1638282040.231713290]: compiled against OGRE version 1.9.0 (Ghadamon) [ INFO] [1638282040.276364970]: Forcing OpenGl version 0. [ INFO] [1638282040.512355947, 1.772000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot [ INFO] [1638282040.530020089, 1.788000000]: IMU: High resolution IMU detected! [ INFO] [1638282041.579478284, 2.820000000]: GF: Using MISSION_ITEM_INT [ INFO] [1638282041.579945650, 2.820000000]: RP: Using MISSION_ITEM_INT [ INFO] [1638282041.580108864, 2.820000000]: WP: Using MISSION_ITEM_INT [ INFO] [1638282041.580235624, 2.820000000]: VER: 1.1: Capabilities 0x000000000000e4ef [ INFO] [1638282041.580339388, 2.820000000]: VER: 1.1: Flight software: 010c03ff (2e8918da66000000) [ INFO] [1638282041.580433132, 2.820000000]: VER: 1.1: Middleware software: 010c03ff (2e8918da66000000) [ INFO] [1638282041.580525910, 2.820000000]: VER: 1.1: OS software: 050400ff (0000000000000000) [ INFO] [1638282041.580607882, 2.820000000]: VER: 1.1: Board hardware: 00000001 [ INFO] [1638282041.580694005, 2.820000000]: VER: 1.1: VID/PID: 0000:0000 [ INFO] [1638282041.580771223, 2.820000000]: VER: 1.1: UID: 4954414c44494e4f [ INFO] [1638282042.726994589, 3.900000000]: Stereo is NOT SUPPORTED [ INFO] [1638282042.727376483, 3.900000000]: OpenGL device: AMD KABINI (DRM 2.50.0, 5.4.0-91-generic, LLVM 10.0.0) [ INFO] [1638282042.727521040, 3.900000000]: OpenGl version: 4.5 (GLSL 4.5) limited to GLSL 1.4 on Mesa system. INFO [ecl/EKF] 4088000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) [ INFO] [1638282043.052167677, 4.216000000]: IMU: Attitude quaternion IMU detected! INFO [ecl/EKF] 4136000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 6208000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 6216000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 6240000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 6256000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) [ INFO] [1638282051.198365304, 11.776000000]: HP: requesting home position [ INFO] [1638282056.767455113, 16.772000000]: GF: mission received [ INFO] [1638282056.770973389, 16.776000000]: RP: mission received [ INFO] [1638282056.772186251, 16.776000000]: WP: mission received [ INFO] [1638282062.524247069, 21.776000000]: HP: requesting home position [ INFO] [1638282073.293087079, 31.776000000]: HP: requesting home position [ INFO] [1638282083.913774525, 41.776000000]: HP: requesting home position [ INFO] [1638282094.581061154, 51.776000000]: HP: requesting home position [ INFO] [1638282105.524312825, 61.776000000]: HP: requesting home position [ INFO] [1638282116.349969135, 71.776000000]: HP: requesting home position [WARN] [1638282119.546471, 74.736000]: Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details. [ INFO] [1638282127.239251920, 81.776000000]: HP: requesting home position [ INFO] [1638282137.474150592, 91.776000000]: HP: requesting home position [ INFO] [1638282147.687610585, 101.776000000]: HP: requesting home position

  1. Then I run, 'px4_rrt_avoidance Master_node.py' I get the following printed on terminal: boon@boon-Vostro-3445:~/catkin_ws/src/px4_rrt_avoidance/launch$ rosrun px4_rrt_avoidance Master_node.py waiting for Service... finished! [INFO] [1638282117.039735, 72.456000]: UAV velocity max: 5.0 [INFO] [1638282117.044739, 72.460000]: Max. acceleration: 3.0

    map was nut fully updated yet [0, 0, 4] rrt empty On Track True mission wp free [0, 0, 4] start_p [0, 0] end_p [0, 0, 4] Traceback (most recent call last): File "/home/boon/catkin_ws/src/px4_rrt_avoidance/nodes/Master_node.py", line 1433, in drone_1.follow_path() File "/home/boon/catkin_ws/src/px4_rrt_avoidance/nodes/Master_node.py", line 783, in follow_path msg = self.setTarget2NextWP(msg) File "/home/boon/catkin_ws/src/px4_rrt_avoidance/nodes/Master_node.py", line 594, in setTarget2NextWP msg.point_1.yaw = calc_direction(next_wp) File "/home/boon/catkin_ws/src/px4_rrt_avoidance/nodes/Master_node.py", line 908, in calc_direction return drone_1.position[5] IndexError: list index out of range

Lars-Ki commented 2 years ago

My initial guess is that something is wrong with the local_map_publisher. But it is running apparently. Do you have any local changes, because you are running:
roslaunch px4_rrt_avoidance iris_depth_wo_rviz.launch

When Gazebo launches, is there an obstacle close to the drone? An indication of a missing obstacle is this line of your summary:
[ WARN] [1638282036.558534467]: Nothing to publish, octree is empty

For the local_map_publisher to work, the 'projected_map'-topic must be published. This only happens when an obstacle is seen by the D435 stereo camera (range is <10 meters)

boonflies commented 2 years ago

i had set gazebo gui to false in the updated launch file(iris_depth_wo_rviz.launch),

I used the original launch file and took a screenshot of the gazebo world and pasted below: iris_world n

I also tried to move the wall very close to the quad (at postition 0.999) but still got the error.

Also, i don't get the topic /mavros/local_position/pose

when i try, rostopic echo /mavros/local_position/pose , I get the output, WARNING: no messages received and simulated time is active. Is /clock being published?

So, probably without the pose, octomap server is not creating the map. But I am not sure why the mavros topic is not published.

Lars-Ki commented 2 years ago

This sounds like the connection is not set up properly. Do you have a connection with QGroundControl? Could you try: rostopic echo /mavros/state

If you are not connected have a look at this.

boonflies commented 2 years ago

When i try the command - rostopic echo /mavros/state, it prints,

header: seq: 1033 stamp: secs: 1036 nsecs: 232000000 frame_id: '' connected: True armed: False guided: False manual_input: True mode: "MANUAL" system_status: 3

Lars-Ki commented 2 years ago

Could you give me the output of the following commands:

rostopic list

and

rosnode list

boonflies commented 2 years ago

boon@boon-Vostro-3445:~$ rosnode list /camera_static_tf /companion_checker /drone_in_map /dynamic_parmater /gazebo /gazebo_gui /local_map_publisher /local_planner /mavros /octomap_server /px4_rrt_avoidance_tf /rosout /rostopic_20206_1638289236959 /rviz

boon@boon-Vostro-3445:~$ rostopic list /camera/depth/points /clicked_point /clock /diagnostics /drone_map /dynamic_obstacle /followed_path /free_cells_vis_array /gazebo/link_states /gazebo/model_states /gazebo/parameter_descriptions /gazebo/parameter_updates /gazebo/set_link_state /gazebo/set_model_state /gazebo/true_pose /initialpose /local_map /local_map_updates /mavlink/from /mavlink/gcs_ip /mavlink/to /mavros/actuator_control /mavros/adsb/send /mavros/adsb/vehicle /mavros/altitude /mavros/battery /mavros/battery2 /mavros/cam_imu_sync/cam_imu_stamp /mavros/companion_process/status /mavros/debug_value/debug /mavros/debug_value/debug_vector /mavros/debug_value/named_value_float /mavros/debug_value/named_value_int /mavros/debug_value/send /mavros/esc_info /mavros/esc_status /mavros/esc_telemetry /mavros/estimator_status /mavros/extended_state /mavros/fake_gps/mocap/tf /mavros/geofence/waypoints /mavros/global_position/compass_hdg /mavros/global_position/global /mavros/global_position/gp_lp_offset /mavros/global_position/gp_origin /mavros/global_position/home /mavros/global_position/local /mavros/global_position/raw/fix /mavros/global_position/raw/gps_vel /mavros/global_position/raw/satellites /mavros/global_position/rel_alt /mavros/global_position/set_gp_origin /mavros/gps_input/gps_input /mavros/gps_rtk/rtk_baseline /mavros/gps_rtk/send_rtcm /mavros/gpsstatus/gps1/raw /mavros/gpsstatus/gps1/rtk /mavros/gpsstatus/gps2/raw /mavros/gpsstatus/gps2/rtk /mavros/hil/actuator_controls /mavros/hil/controls /mavros/hil/gps /mavros/hil/imu_ned /mavros/hil/optical_flow /mavros/hil/rc_inputs /mavros/hil/state /mavros/home_position/home /mavros/home_position/set /mavros/imu/data /mavros/imu/data_raw /mavros/imu/diff_pressure /mavros/imu/mag /mavros/imu/static_pressure /mavros/imu/temperature_baro /mavros/imu/temperature_imu /mavros/landing_target/lt_marker /mavros/landing_target/pose /mavros/landing_target/pose_in /mavros/local_position/accel /mavros/local_position/odom /mavros/local_position/pose /mavros/local_position/pose_cov /mavros/local_position/velocity_body /mavros/local_position/velocity_body_cov /mavros/local_position/velocity_local /mavros/log_transfer/raw/log_data /mavros/log_transfer/raw/log_entry /mavros/mag_calibration/report /mavros/mag_calibration/status /mavros/manual_control/control /mavros/manual_control/send /mavros/mission/reached /mavros/mission/waypoints /mavros/mocap/pose /mavros/mount_control/command /mavros/mount_control/orientation /mavros/mount_control/status /mavros/nav_controller_output /mavros/obstacle/send /mavros/odometry/in /mavros/odometry/out /mavros/onboard_computer/status /mavros/param/param_value /mavros/play_tune /mavros/px4flow/ground_distance /mavros/px4flow/raw/optical_flow_rad /mavros/px4flow/raw/send /mavros/px4flow/temperature /mavros/radio_status /mavros/rallypoint/waypoints /mavros/rc/in /mavros/rc/out /mavros/rc/override /mavros/setpoint_accel/accel /mavros/setpoint_attitude/cmd_vel /mavros/setpoint_attitude/thrust /mavros/setpoint_position/global /mavros/setpoint_position/global_to_local /mavros/setpoint_position/local /mavros/setpoint_raw/attitude /mavros/setpoint_raw/global /mavros/setpoint_raw/local /mavros/setpoint_raw/target_attitude /mavros/setpoint_raw/target_global /mavros/setpoint_raw/target_local /mavros/setpoint_trajectory/desired /mavros/setpoint_trajectory/local /mavros/setpoint_velocity/cmd_vel /mavros/setpoint_velocity/cmd_vel_unstamped /mavros/state /mavros/statustext/recv /mavros/statustext/send /mavros/target_actuator_control /mavros/time_reference /mavros/timesync_status /mavros/trajectory/desired /mavros/trajectory/generated /mavros/trajectory/path /mavros/tunnel/in /mavros/tunnel/out /mavros/vfr_hud /mavros/vision_pose/pose /mavros/vision_pose/pose_cov /mavros/vision_speed/speed_twist_cov /mavros/wind_estimation /maximize_map /move_base_simple/goal /occupied_cells_vis_array /octomap_binary /octomap_full /octomap_point_cloud_centers /octomap_server/parameter_descriptions /octomap_server/parameter_updates /path_map /path_map_updates /projected_map /projected_map_updates /rosout /rosout_agg /safe_path /start_planning /suggested_path /tf /tf_static

Lars-Ki commented 2 years ago

One thing that I noticed in the image you posted was that the quadcopter is missing the camera on top (white block about 10 cm above the drone). It seems that everything else is loading fine.
Just to be sure can you see the camera? It looks like this:

iris_D435

Also, please check the topic rostopic echo /camera/depth/points
Does it publish anything?

And please run rqt_graph after roslaunch. The results should look like this: rqt_graph_min

And the only thing you changed in the launch-file was Line 6 to false correct?

boonflies commented 2 years ago

Yes, you were right with the camera. There was a conflict with another iris model that i had. I correct that and now I am receiving messages in the topic - /camera/depth/points. iris_with_camera

The rqt_graph shows: iris_rqt_term1

But still, i don't get any messages on the topic - mavros/local_position/pose.

The terminal #1 output: boon@boon-Vostro-3445:~$ roslaunch px4_rrt_avoidance iris_depth.launch ... logging to /home/boon/.ros/log/e47fd7c2-522f-11ec-8130-c03896420cbf/roslaunch-boon-Vostro-3445-4194.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt WARNING: disk usage in log directory [/home/boon/.ros/log] is over 1GB. It's recommended that you use the 'rosclean' command.

started roslaunch server http://boon-Vostro-3445:36365/

SUMMARY

CLEAR PARAMETERS

PARAMETERS

NODES / camera_static_tf (tf/static_transform_publisher) companion_checker (px4_rrt_avoidance/companion_computer.py) drone_in_map (px4_rrt_avoidance/drone_in_local_map.py) dynamic_parmater (px4_rrt_avoidance/dynamic_param_reconfigure.py) gazebo (gazebo_ros/gzserver) local_map_publisher (px4_rrt_avoidance/local_map_publisher.py) local_planner (px4_rrt_avoidance/local_planner.py) mavros (mavros/mavros_node) octomap_server (octomap_server/octomap_server_node) px4_rrt_avoidance_tf (px4_rrt_avoidance/FC_pose_tf_publisher.py) rviz (rviz/rviz) sitl_0 (px4/px4)

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

setting /run_id to e47fd7c2-522f-11ec-8130-c03896420cbf process[rosout-1]: started with pid [4257] started core service [/rosout] process[gazebo-2]: started with pid [4261] process[mavros-3]: started with pid [4268] [ INFO] [1638312634.433878213]: FCU URL: udp://:14540@localhost:14557 [ INFO] [1638312634.496378363]: udp0: Bind address: 0.0.0.0:14540 [ INFO] [1638312634.496885145]: udp0: Remote address: 127.0.0.1:14557 [ INFO] [1638312634.497964123]: GCS bridge disabled [ INFO] [1638312639.491649377]: Plugin 3dr_radio loaded [ INFO] [1638312639.497922867]: Plugin 3dr_radio initialized [ INFO] [1638312639.498440242]: Plugin actuator_control loaded [ INFO] [1638312639.507012273]: Plugin actuator_control initialized [ INFO] [1638312642.486881309]: Plugin adsb loaded [ INFO] [1638312642.496628196]: Plugin adsb initialized [ INFO] [1638312642.497132449]: Plugin altitude loaded [ INFO] [1638312642.500130938]: Plugin altitude initialized [ INFO] [1638312642.500602210]: Plugin cam_imu_sync loaded [ INFO] [1638312642.503108086]: Plugin cam_imu_sync initialized [ INFO] [1638312642.503798118]: Plugin command loaded [ INFO] [1638312642.525892314]: Plugin command initialized [ INFO] [1638312642.526415395]: Plugin companion_process_status loaded [ INFO] [1638312642.532595016]: Plugin companion_process_status initialized [ INFO] [1638312642.533144401]: Plugin debug_value loaded [ INFO] [1638312642.543316113]: Plugin debug_value initialized [ INFO] [1638312642.543434162]: Plugin distance_sensor blacklisted [ INFO] [1638312642.543923038]: Plugin esc_status loaded [ INFO] [1638312642.547832564]: Plugin esc_status initialized [ INFO] [1638312642.548368596]: Plugin esc_telemetry loaded [ INFO] [1638312642.551191995]: Plugin esc_telemetry initialized [ INFO] [1638312642.551757755]: Plugin fake_gps loaded [ INFO] [1638312642.585138954]: Plugin fake_gps initialized [ INFO] [1638312642.585747939]: Plugin ftp loaded [ INFO] [1638312642.657850353]: Plugin ftp initialized [ INFO] [1638312642.658470052]: Plugin geofence loaded [ INFO] [1638312642.668498783]: Plugin geofence initialized [ INFO] [1638312642.669164592]: Plugin global_position loaded [ INFO] [1638312642.707208223]: Plugin global_position initialized [ INFO] [1638312642.707670218]: Plugin gps_input loaded [ INFO] [1638312642.714985928]: Plugin gps_input initialized [ INFO] [1638312642.715508047]: Plugin gps_rtk loaded [ INFO] [1638312642.724545021]: Plugin gps_rtk initialized [ INFO] [1638312642.725017570]: Plugin gps_status loaded [ INFO] [1638312642.731448349]: Plugin gps_status initialized [ INFO] [1638312642.733011698]: Plugin hil loaded [ INFO] [1638312642.764264082]: Plugin hil initialized [ INFO] [1638312642.764892783]: Plugin home_position loaded [ INFO] [1638312642.775232251]: Plugin home_position initialized [ INFO] [1638312642.776022518]: Plugin imu loaded [ INFO] [1638312642.796080271]: Plugin imu initialized [ INFO] [1638312642.796581179]: Plugin landing_target loaded [ INFO] [1638312642.889827188]: Plugin landing_target initialized [ INFO] [1638312642.890472521]: Plugin local_position loaded [ INFO] [1638312642.909173294]: Plugin local_position initialized [ INFO] [1638312642.909695400]: Plugin log_transfer loaded [ INFO] [1638312642.921443713]: Plugin log_transfer initialized [ INFO] [1638312642.922002861]: Plugin mag_calibration_status loaded [ INFO] [1638312642.925509263]: Plugin mag_calibration_status initialized [ INFO] [1638312642.926096825]: Plugin manual_control loaded [ INFO] [1638312642.933769245]: Plugin manual_control initialized [ INFO] [1638312642.934259504]: Plugin mocap_pose_estimate loaded [ INFO] [1638312642.941730220]: Plugin mocap_pose_estimate initialized [ INFO] [1638312642.942190125]: Plugin mount_control loaded [ INFO] [1638312642.952510393]: Plugin mount_control initialized [ INFO] [1638312642.953130648]: Plugin nav_controller_output loaded [ INFO] [1638312642.955506516]: Plugin nav_controller_output initialized [ INFO] [1638312642.955947572]: Plugin obstacle_distance loaded [ INFO] [1638312642.962420175]: Plugin obstacle_distance initialized [ INFO] [1638312642.962960420]: Plugin odom loaded [ INFO] [1638312643.003428252]: Plugin odom initialized [ INFO] [1638312643.003920726]: Plugin onboard_computer_status loaded [ INFO] [1638312643.009124768]: Plugin onboard_computer_status initialized [ INFO] [1638312643.009816094]: Plugin param loaded [ INFO] [1638312643.020714025]: Plugin param initialized [ INFO] [1638312643.021319849]: Plugin play_tune loaded [ INFO] [1638312643.027508414]: Plugin play_tune initialized [ INFO] [1638312643.028027911]: Plugin px4flow loaded [ INFO] [1638312643.043737083]: Plugin px4flow initialized [ INFO] [1638312643.044301802]: Plugin rallypoint loaded [ INFO] [1638312643.053028109]: Plugin rallypoint initialized [ INFO] [1638312643.053206802]: Plugin rangefinder blacklisted [ INFO] [1638312643.053789241]: Plugin rc_io loaded [ INFO] [1638312643.062241977]: Plugin rc_io initialized [ INFO] [1638312643.062366282]: Plugin safety_area blacklisted [ INFO] [1638312643.062879840]: Plugin setpoint_accel loaded [ INFO] [1638312643.071578885]: Plugin setpoint_accel initialized [ INFO] [1638312643.072402367]: Plugin setpoint_attitude loaded [ INFO] [1638312643.163501735]: Plugin setpoint_attitude initialized [ INFO] [1638312643.164503715]: Plugin setpoint_position loaded [ INFO] [1638312643.204770158]: Plugin setpoint_position initialized [ INFO] [1638312643.205354589]: Plugin setpoint_raw loaded [ INFO] [1638312643.227742921]: Plugin setpoint_raw initialized [ INFO] [1638312643.228438995]: Plugin setpoint_trajectory loaded [ INFO] [1638312643.240170533]: Plugin setpoint_trajectory initialized [ INFO] [1638312643.240728777]: Plugin setpoint_velocity loaded [ INFO] [1638312643.252857525]: Plugin setpoint_velocity initialized [ INFO] [1638312643.253986871]: Plugin sys_status loaded [ INFO] [1638312643.317786798]: Plugin sys_status initialized [ INFO] [1638312643.318402749]: Plugin sys_time loaded [ INFO] [1638312643.329890011]: TM: Timesync mode: MAVLINK [ INFO] [1638312643.331327745]: TM: Not publishing sim time [ INFO] [1638312643.335640282]: Plugin sys_time initialized [ INFO] [1638312643.336158843]: Plugin trajectory loaded [ INFO] [1638312643.349777330]: Plugin trajectory initialized [ INFO] [1638312643.350327883]: Plugin tunnel loaded [ INFO] [1638312643.364344305]: Plugin tunnel initialized [ INFO] [1638312643.365089541]: Plugin vfr_hud loaded [ INFO] [1638312643.367616740]: Plugin vfr_hud initialized [ INFO] [1638312643.367743497]: Plugin vibration blacklisted [ INFO] [1638312643.368176972]: Plugin vision_pose_estimate loaded [ INFO] [1638312643.386186343]: Plugin vision_pose_estimate initialized [ INFO] [1638312643.386770695]: Plugin vision_speed_estimate loaded [ INFO] [1638312643.395621668]: Plugin vision_speed_estimate initialized [ INFO] [1638312643.396191969]: Plugin waypoint loaded [ INFO] [1638312643.408117417]: Plugin waypoint initialized [ INFO] [1638312643.408293782]: Plugin wheel_odometry blacklisted [ INFO] [1638312643.408794084]: Plugin wind_estimation loaded [ INFO] [1638312643.412505283]: Plugin wind_estimation initialized [ INFO] [1638312643.413077667]: Autostarting mavlink via USB on PX4 [ INFO] [1638312643.413505490]: Built-in SIMD instructions: SSE, SSE2 [ INFO] [1638312643.413584951]: Built-in MAVLink package version: 2021.3.3 [ INFO] [1638312643.413673118]: Known MAVLink dialects: common ardupilotmega ASLUAV all autoquad icarous matrixpilot paparazzi standard uAvionix ualberta [ INFO] [1638312643.413777194]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [ INFO] [1638312662.364696717]: Finished loading Gazebo ROS API Plugin. [ INFO] [1638312662.369470271]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting... process[sitl_0-4]: started with pid [4360] process[px4_rrt_avoidance_tf-5]: started with pid [4365] INFO [px4] Creating symlink /home/boon/src/PX4-Autopilot/build/px4_sitl_default/etc -> /home/boon/.ros/etc process[dynamic_parmater-6]: started with pid [4366]

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

px4 starting.

INFO [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0 process[camera_static_tf-7]: started with pid [4384] process[octomap_server-8]: started with pid [4388] process[rviz-9]: started with pid [4401] process[drone_in_map-10]: started with pid [4402] Info: found model autostart file as SYS_AUTOSTART=10016 process[local_planner-11]: started with pid [4403] INFO [param] selected parameter default file eeprom/parameters_10016 process[local_map_publisher-12]: started with pid [4406] process[companion_checker-13]: started with pid [4409] INFO [parameters] BSON document size 423 bytes, decoded 423 bytes [param] Loaded: eeprom/parameters_10016 INFO [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes PX4 SIM HOST: localhost INFO [simulator] Waiting for simulator to accept connection on TCP port 4560 [ INFO] [1638312670.239612949]: waitForService: Service [/gazebo/set_physics_properties] is now available. [ INFO] [1638312670.686764075]: Physics dynamic reconfigure ready. INFO: cannot create a symlink to latest log directory: [Errno 17] File exists [ INFO] [1638312675.396653619]: Camera Plugin: The 'robotNamespace' param did not exit [ INFO] [1638312675.587535021]: Camera Plugin (ns = ) , set to "" INFO: cannot create a symlink to latest log directory: [Errno 2] No such file or directory: '/home/boon/.ros/log/latest' [ WARN] [1638312679.182341518]: Nothing to publish, octree is empty [ INFO] [1638312681.454057165]: rviz version 1.13.21 [ INFO] [1638312681.454209653]: compiled against Qt version 5.9.5 [ INFO] [1638312681.454270312]: compiled against OGRE version 1.9.0 (Ghadamon) [ INFO] [1638312681.480002812]: Forcing OpenGl version 0. INFO [simulator] Simulator connected on TCP port 4560. INFO [commander] LED: open /dev/led0 failed (22) INFO [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0 INFO [ekf2] starting instance 0, IMU:0 (1310988), MAG:0 (197388) INFO [ekf2] starting instance 1, IMU:1 (1310996), MAG:0 (197388) INFO [ekf2] starting instance 2, IMU:2 (1311004), MAG:0 (197388) INFO [ekf2] starting instance 3, IMU:0 (1310988), MAG:1 (197644) INFO [ekf2] starting instance 4, IMU:1 (1310996), MAG:1 (197644) INFO [ekf2] starting instance 5, IMU:2 (1311004), MAG:1 (197644) INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550 INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540 INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030 INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [ecl/EKF] reset position to last known position INFO [ecl/EKF] reset velocity to zero INFO [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280 INFO [logger] logger started (mode=all) [ INFO] [1638312684.056502260, 1.432000000]: udp0: Remote address: 127.0.0.1:14580 [ INFO] [1638312684.058820188, 1.432000000]: IMU: High resolution IMU detected! INFO [logger] Start file log (type: full) INFO [logger] [logger] ./log/2021-11-30/22_5124.ulg INFO [mavlink] MAVLink only on localhost (set param MAV{i}BROADCAST = 1 to enable network) INFO [mavlink] MAVLink only on localhost (set param MAV{i}_BROADCAST = 1 to enable network) INFO [px4] Startup script returned successfully pxh> INFO [mavlink] partner IP: 127.0.0.1 INFO [logger] Opened full log file: ./log/2021-11-30/22_51_24.ulg WARN [mavlink] [timesync] RTT too high for timesync: 56 ms (sender: 240) [ WARN] [1638312684.840823576, 2.160000000]: TM : RTT too high for timesync: 60.00 ms. [ INFO] [1638312684.998448888, 2.312000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot [ INFO] [1638312685.007178268, 2.320000000]: IMU: High resolution IMU detected! [ INFO] [1638312685.912634299, 3.108000000]: Stereo is NOT SUPPORTED [ INFO] [1638312685.912821142, 3.108000000]: OpenGL device: AMD KABINI (DRM 2.50.0, 5.4.0-91-generic, LLVM 10.0.0) [ INFO] [1638312685.912905449, 3.108000000]: OpenGl version: 4.5 (GLSL 4.5) limited to GLSL 1.4 on Mesa system. [ INFO] [1638312686.127614009, 3.324000000]: GF: Using MISSION_ITEM_INT [ INFO] [1638312686.127786370, 3.324000000]: RP: Using MISSION_ITEM_INT [ INFO] [1638312686.127883439, 3.324000000]: WP: Using MISSION_ITEM_INT [ INFO] [1638312686.128328050, 3.324000000]: VER: 1.1: Capabilities 0x000000000000e4ef [ INFO] [1638312686.128966238, 3.324000000]: VER: 1.1: Flight software: 010c03ff (2e8918da66000000) [ INFO] [1638312686.129088557, 3.324000000]: VER: 1.1: Middleware software: 010c03ff (2e8918da66000000) [ INFO] [1638312686.129262790, 3.324000000]: VER: 1.1: OS software: 050400ff (0000000000000000) [ INFO] [1638312686.129462403, 3.324000000]: VER: 1.1: Board hardware: 00000001 [ INFO] [1638312686.129767224, 3.324000000]: VER: 1.1: VID/PID: 0000:0000 [ INFO] [1638312686.129929015, 3.324000000]: VER: 1.1: UID: 4954414c44494e4f INFO [ecl/EKF] 4740000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 4744000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 6400000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) [ INFO] [1638312690.103818625, 6.528000000]: IMU: Attitude quaternion IMU detected! INFO [ecl/EKF] 6416000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 6456000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) INFO [ecl/EKF] 6464000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9) [ INFO] [1638312697.831390915, 12.380000000]: HP: requesting home position [ WARN] [1638312701.195811310, 15.016000000]: MessageFilter [target=map ]: Dropped 100.00% of messages so far. Please turn the [ros.octomap_server.message_filter] rosconsole logger to DEBUG for more information. [ INFO] [1638312704.235566658, 17.452000000]: RP: mission received [ INFO] [1638312704.236911038, 17.452000000]: WP: mission received [ INFO] [1638312704.237966591, 17.452000000]: GF: mission received [ INFO] [1638312710.876324355, 22.316000000]: HP: requesting home position [ INFO] [1638312723.510053989, 32.312000000]: HP: requesting home position [ INFO] [1638312736.883910021, 42.312000000]: HP: requesting home position [ INFO] [1638312749.558776556, 52.312000000]: HP: requesting home position [ERROR] [1638312751.027061138, 53.500000000]: TM : Time jump detected. Resetting time synchroniser. [ INFO] [1638312763.156860799, 62.320000000]: HP: requesting home position [ INFO] [1638312776.323171523, 72.312000000]: HP: requesting home position [ WARN] [1638312779.962453166, 75.036000000]: MessageFilter [target=map ]: Dropped 100.00% of messages so far. Please turn the [ros.octomap_server.message_filter] rosconsole logger to DEBUG for more information. [ INFO] [1638312789.361387712, 82.312000000]: HP: requesting home position [ INFO] [1638312802.570497632, 92.312000000]: HP: requesting home position [ INFO] [1638312815.619065936, 102.312000000]: HP: requesting home position [ERROR] [1638312819.271129768, 105.400000000]: TM : Time jump detected. Resetting time synchroniser. [ INFO] [1638312827.899338538, 112.312000000]: HP: requesting home position [ INFO] [1638312840.177532241, 122.312000000]: HP: requesting home position

The terminal #2 (rosrun px4_rrt_avoidance Master_node.py) returns the same error.

Also, the tf warning appears in rviz, iris_rviz

Could this be due to gps info not being published? Because, I tried a reinstallation of ROS, Mavros related files and gazebo, to check if there was an error with the plugins. But after that update, i am getting segmentation fault while loading iris in gazebo when 'libgazebo_gps_plugin.so' is included in 'iris.sdf'. (The rviz warning appeared even before the update).

Lars-Ki commented 2 years ago

Your rqt_graph does not show the /tf-topic. Mavros does not publish any transformation information. I am not sure why.
It should look like this (with master_node running): rqt_graph_max

I assume you have to fix the tf problem first. Once you fix that, it should work.

Lars-Ki commented 2 years ago

I might found your issue: /mavros/local_position/tf/send: False try the solution here.

Lars-Ki commented 1 year ago

No activity. Closing Issue