mavros_apriltag_tracking
mavros_apriltag_tracking copied to clipboard
mzahana
Filter is not initialized. Requires a measurement first.
Hi @mzahana, thanks for sharing the outstanding work. I' m trying to run your package on my computer, but met the following problem:
......
INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14558 remote port 14530
[ WARN] [1606965215.625816867, 1350.060000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1606965215.625959064, 1350.060000000]: Failed to meet update rate! Took 1350.0599999999999454
[ WARN] [1606965215.626680237, 1350.060000000]: Failed to meet update rate! Took 1350.0399999999999636
[INFO] [1606965215.639626, 1350.072000]: Calling service /gazebo/spawn_sdf_model
INFO [logger] logger started (mode=all)
INFO [logger] Start file log (type: full)
INFO [logger] Opened full log file: ./log/2020-12-03/03_13_35.ulg
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
[ INFO] [1606965215.650181957, 1350.084000000]: udp0: Remote address: 127.0.0.1:14580
[ WARN] [1606965215.650418862, 1350.084000000]: GP: No GPS fix
[ INFO] [1606965215.650515303, 1350.084000000]: FCU: [logger] file: ./log/2020-12-03/03_13_35.ulg
[ INFO] [1606965215.650568810, 1350.084000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
INFO [px4] Startup script returned successfully
pxh> INFO [mavlink] partner IP: 127.0.0.1
Warning [parser_urdf.cc:1115] multiple inconsistent
Exiting... pxh> Shutting down [tf_world_to_map-3] killing on exit [tf_drone_to_camera-2] killing on exit [gazebo_gui-7] escalating to SIGTERM [gazebo-6] escalating to SIGTERM [rosout-1] killing on exit [master] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done
The uav can takeoff and the Husky also can follow it's path. I echo the rostopic of the "/cgo3_camera/image_raw" and there is no publisher for this topic. Is this the reason for this problem?
rostopic info /cgo3_camera/image_raw Type: sensor_msgs/Image
Publishers: None
Subscribers:
- /apriltag_ros_continuous_node (http://lxl:36069/)
Any reply will be appreciated
Hi @zsyfight
Did you copy the folder of the modified typhoon_h480 model from the package to the Firmware/Tools/sitl_gazebo/models/ ?
cp -R ~/catkin_ws/src/mavros_apriltag_tracking/models/typhoon_h480 ~/Firmware/Tools/sitl_gazebo/models/
Also, don't forget to copy the modified Husky simulation model
sudo cp ~/catkin_ws/src/mavros_apriltag_tracking/models/custom_husky/husky.urdf.xacro $(catkin_find husky_description/urdf)/
Those steps are mentioned in the README file, too.
Let me know if it solves your issue.
Thanks for your quickly response. I have done the copy operations as stated in the README file. There still appear this error. And i can make sure the robots and the camera are loaded in the gazebo world, as shown in the following:
Do you have some ideas about this problem? Thanks again for you response.
Can you set the verbose argument in mavros.launch to true and see if there are errors shown on the terminal (https://github.com/mzahana/mavros_apriltag_tracking/blob/master/launch/mavros.launch#L17)
Also can you show which nodes are running ? rosnode list
I set the verbose to true, and there is the information on the terminal:
roslaunch mavros_apriltag_tracking tracker.launch
... logging to /home/hzx/.ros/log/5b4c2b1a-3870-11eb-b692-b025aa2fdbf4/roslaunch-hzx-23473.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.
Deprecated: xacro tag 'sick_lms1xx_mount' w/o 'xacro:' xml namespace prefix (will be forbidden in Noetic)
when processing file: /opt/ros/melodic/share/husky_description/urdf/husky.urdf.xacro
Use the following command to fix incorrect tag usage:
find . -iname "*.xacro" | xargs sed -i 's#<\([/]\?\)\(if\|unless\|include\|arg\|property\|macro\|insert_block\)#<\1xacro:\2#g'
started roslaunch server http://hzx:38753/
SUMMARY
========
CLEAR PARAMETERS
* /apriltag_ros_continuous_node/
* /mavros/
PARAMETERS
* /apriltag_ros_continuous_node/camera_frame: cgo3_camera_optic...
* /apriltag_ros_continuous_node/publish_tag_detections_image: True
* /apriltag_ros_continuous_node/publish_tf: True
* /apriltag_ros_continuous_node/standalone_tags: [{'size': 1.0, 'i...
* /apriltag_ros_continuous_node/tag_blur: 0.0
* /apriltag_ros_continuous_node/tag_bundles: []
* /apriltag_ros_continuous_node/tag_debug: 0
* /apriltag_ros_continuous_node/tag_decimate: 1.0
* /apriltag_ros_continuous_node/tag_family: tag36h11
* /apriltag_ros_continuous_node/tag_refine_edges: 1
* /apriltag_ros_continuous_node/tag_threads: 2
* /apriltag_setpoint_publisher_node/alt_from_tag: 3.0
* /apriltag_setpoint_publisher_node/drone_frame_id: /drone_link
* /apriltag_setpoint_publisher_node/setpoint_topic: setpoint/relative...
* /apriltag_setpoint_publisher_node/tag_frame_id: /tag_0
* /apriltag_setpoint_publisher_node/tag_id: 0
* /apriltag_setpoint_publisher_node/tags_topic: tag_detections
* /ekf_localization/base_link_frame: base_link
* /ekf_localization/frequency: 50
* /ekf_localization/imu0: imu/data
* /ekf_localization/imu0_config: [False, False, Fa...
* /ekf_localization/imu0_differential: True
* /ekf_localization/imu0_queue_size: 10
* /ekf_localization/imu0_remove_gravitational_acceleration: True
* /ekf_localization/odom0: husky_velocity_co...
* /ekf_localization/odom0_config: [False, False, Fa...
* /ekf_localization/odom0_differential: False
* /ekf_localization/odom0_queue_size: 10
* /ekf_localization/odom_frame: odom
* /ekf_localization/two_d_mode: True
* /ekf_localization/world_frame: odom
* /gazebo/enable_ros_network: True
* /husky_joint_publisher/publish_rate: 50
* /husky_joint_publisher/type: joint_state_contr...
* /husky_velocity_controller/angular/z/has_acceleration_limits: True
* /husky_velocity_controller/angular/z/has_velocity_limits: True
* /husky_velocity_controller/angular/z/max_acceleration: 6.0
* /husky_velocity_controller/angular/z/max_velocity: 2.0
* /husky_velocity_controller/base_frame_id: base_link
* /husky_velocity_controller/cmd_vel_timeout: 0.25
* /husky_velocity_controller/enable_odom_tf: False
* /husky_velocity_controller/estimate_velocity_from_position: False
* /husky_velocity_controller/left_wheel: ['front_left_whee...
* /husky_velocity_controller/linear/x/has_acceleration_limits: True
* /husky_velocity_controller/linear/x/has_velocity_limits: True
* /husky_velocity_controller/linear/x/max_acceleration: 3.0
* /husky_velocity_controller/linear/x/max_velocity: 1.0
* /husky_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
* /husky_velocity_controller/publish_rate: 50
* /husky_velocity_controller/right_wheel: ['front_right_whe...
* /husky_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
* /husky_velocity_controller/type: diff_drive_contro...
* /husky_velocity_controller/velocity_rolling_window_size: 2
* /husky_velocity_controller/wheel_radius_multiplier: 1.0
* /husky_velocity_controller/wheel_separation_multiplier: 1.875
* /joy_teleop/joy_node/autorepeat_rate: 20
* /joy_teleop/joy_node/deadzone: 0.1
* /joy_teleop/joy_node/dev: /dev/input/ps4
* /joy_teleop/teleop_twist_joy/axis_angular: 0
* /joy_teleop/teleop_twist_joy/axis_linear: 1
* /joy_teleop/teleop_twist_joy/enable_button: 4
* /joy_teleop/teleop_twist_joy/enable_turbo_button: 5
* /joy_teleop/teleop_twist_joy/scale_angular: 1.4
* /joy_teleop/teleop_twist_joy/scale_linear: 0.4
* /joy_teleop/teleop_twist_joy/scale_linear_turbo: 2.0
* /kf_tracker_node/do_kf_update_step: True
* /kf_tracker_node/dt_pred: 0.05
* /kf_tracker_node/measurement_off_time: 2.0
* /kf_tracker_node/print_debug_msg: False
* /kf_tracker_node/q_std: 1.0
* /kf_tracker_node/r_std: 0.1
* /kf_tracker_node/tracking_frame_id: drone_link
* /mavros/cmd/use_comp_id_system_control: False
* /mavros/conn/heartbeat_rate: 1.0
* /mavros/conn/system_time_rate: 1.0
* /mavros/conn/timeout: 10.0
* /mavros/conn/timesync_rate: 10.0
* /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
* /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
* /mavros/distance_sensor/hrlv_ez4_pub/id: 0
* /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
* /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
* /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
* /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
* /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
* /mavros/distance_sensor/laser_1_sub/id: 3
* /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
* /mavros/distance_sensor/laser_1_sub/subscriber: True
* /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
* /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
* /mavros/distance_sensor/lidarlite_pub/id: 1
* /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
* /mavros/distance_sensor/lidarlite_pub/send_tf: True
* /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
* /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
* /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
* /mavros/distance_sensor/sonar_1_sub/id: 2
* /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
* /mavros/distance_sensor/sonar_1_sub/subscriber: True
* /mavros/fake_gps/eph: 2.0
* /mavros/fake_gps/epv: 2.0
* /mavros/fake_gps/fix_type: 3
* /mavros/fake_gps/geo_origin/alt: 408.0
* /mavros/fake_gps/geo_origin/lat: 47.3667
* /mavros/fake_gps/geo_origin/lon: 8.55
* /mavros/fake_gps/gps_rate: 5.0
* /mavros/fake_gps/mocap_transform: True
* /mavros/fake_gps/satellites_visible: 5
* /mavros/fake_gps/tf/child_frame_id: fix
* /mavros/fake_gps/tf/frame_id: map
* /mavros/fake_gps/tf/listen: False
* /mavros/fake_gps/tf/rate_limit: 10.0
* /mavros/fake_gps/tf/send: False
* /mavros/fake_gps/use_mocap: True
* /mavros/fake_gps/use_vision: False
* /mavros/fcu_protocol: v2.0
* /mavros/fcu_url: udp://:14540@loca...
* /mavros/gcs_url:
* /mavros/global_position/child_frame_id: drone_link
* /mavros/global_position/frame_id: map
* /mavros/global_position/gps_uere: 1.0
* /mavros/global_position/rot_covariance: 99999.0
* /mavros/global_position/tf/child_frame_id: drone_link
* /mavros/global_position/tf/frame_id: map
* /mavros/global_position/tf/global_frame_id: earth
* /mavros/global_position/tf/send: False
* /mavros/global_position/use_relative_alt: True
* /mavros/image/frame_id: px4flow
* /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
* /mavros/imu/frame_id: drone_link
* /mavros/imu/linear_acceleration_stdev: 0.0003
* /mavros/imu/magnetic_stdev: 0.0
* /mavros/imu/orientation_stdev: 1.0
* /mavros/landing_target/camera/fov_x: 2.0071286398
* /mavros/landing_target/camera/fov_y: 2.0071286398
* /mavros/landing_target/image/height: 480
* /mavros/landing_target/image/width: 640
* /mavros/landing_target/land_target_type: VISION_FIDUCIAL
* /mavros/landing_target/listen_lt: False
* /mavros/landing_target/mav_frame: LOCAL_NED
* /mavros/landing_target/target_size/x: 0.3
* /mavros/landing_target/target_size/y: 0.3
* /mavros/landing_target/tf/child_frame_id: camera_center
* /mavros/landing_target/tf/frame_id: landing_target
* /mavros/landing_target/tf/listen: False
* /mavros/landing_target/tf/rate_limit: 10.0
* /mavros/landing_target/tf/send: True
* /mavros/local_position/frame_id: map
* /mavros/local_position/tf/child_frame_id: drone_link
* /mavros/local_position/tf/frame_id: map
* /mavros/local_position/tf/send: True
* /mavros/local_position/tf/send_fcu: False
* /mavros/mission/pull_after_gcs: True
* /mavros/mission/use_mission_item_int: True
* /mavros/mocap/use_pose: True
* /mavros/mocap/use_tf: False
* /mavros/odometry/fcu/odom_child_id_des: drone_link
* /mavros/odometry/fcu/odom_parent_id_des: map
* /mavros/plugin_blacklist: ['safety_area', '...
* /mavros/plugin_whitelist: []
* /mavros/px4flow/frame_id: px4flow
* /mavros/px4flow/ranger_fov: 0.118682
* /mavros/px4flow/ranger_max_range: 5.0
* /mavros/px4flow/ranger_min_range: 0.3
* /mavros/safety_area/p1/x: 1.0
* /mavros/safety_area/p1/y: 1.0
* /mavros/safety_area/p1/z: 1.0
* /mavros/safety_area/p2/x: -1.0
* /mavros/safety_area/p2/y: -1.0
* /mavros/safety_area/p2/z: -1.0
* /mavros/setpoint_accel/send_force: False
* /mavros/setpoint_attitude/reverse_thrust: False
* /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
* /mavros/setpoint_attitude/tf/frame_id: map
* /mavros/setpoint_attitude/tf/listen: False
* /mavros/setpoint_attitude/tf/rate_limit: 50.0
* /mavros/setpoint_attitude/use_quaternion: False
* /mavros/setpoint_position/mav_frame: LOCAL_NED
* /mavros/setpoint_position/tf/child_frame_id: target_position
* /mavros/setpoint_position/tf/frame_id: map
* /mavros/setpoint_position/tf/listen: False
* /mavros/setpoint_position/tf/rate_limit: 50.0
* /mavros/setpoint_raw/thrust_scaling: 1.0
* /mavros/setpoint_velocity/mav_frame: LOCAL_NED
* /mavros/startup_px4_usb_quirk: True
* /mavros/sys/disable_diag: False
* /mavros/sys/min_voltage: 10.0
* /mavros/target_component_id: 1
* /mavros/target_system_id: 1
* /mavros/tdr_radio/low_rssi: 40
* /mavros/time/time_ref_source: fcu
* /mavros/time/timesync_avg_alpha: 0.6
* /mavros/time/timesync_mode: MAVLINK
* /mavros/vibration/frame_id: drone_link
* /mavros/vision_pose/tf/child_frame_id: vision_estimate
* /mavros/vision_pose/tf/frame_id: odom
* /mavros/vision_pose/tf/listen: False
* /mavros/vision_pose/tf/rate_limit: 10.0
* /mavros/vision_speed/listen_twist: True
* /mavros/vision_speed/twist_cov: True
* /mavros/wheel_odometry/child_frame_id: drone_link
* /mavros/wheel_odometry/count: 2
* /mavros/wheel_odometry/frame_id: odom
* /mavros/wheel_odometry/send_raw: True
* /mavros/wheel_odometry/send_twist: False
* /mavros/wheel_odometry/tf/child_frame_id: drone_link
* /mavros/wheel_odometry/tf/frame_id: odom
* /mavros/wheel_odometry/tf/send: False
* /mavros/wheel_odometry/use_rpm: False
* /mavros/wheel_odometry/vel_error: 0.1
* /mavros/wheel_odometry/wheel0/radius: 0.05
* /mavros/wheel_odometry/wheel0/x: 0.0
* /mavros/wheel_odometry/wheel0/y: -0.15
* /mavros/wheel_odometry/wheel1/radius: 0.05
* /mavros/wheel_odometry/wheel1/x: 0.0
* /mavros/wheel_odometry/wheel1/y: 0.15
* /mavros_offboard_controller_node/horizontal_controller/kI: 0.02
* /mavros_offboard_controller_node/horizontal_controller/kP: 1.3
* /mavros_offboard_controller_node/horizontal_controller/vMAX: 2.0
* /mavros_offboard_controller_node/vertical_controller/kI: 0.01
* /mavros_offboard_controller_node/vertical_controller/kP: 2.0
* /mavros_offboard_controller_node/vertical_controller/vDownMAX: 0.5
* /mavros_offboard_controller_node/vertical_controller/vUpMAX: 1.0
* /robot_description: <?xml version="1....
* /rosdistro: melodic
* /rosversion: 1.14.9
* /twist_mux/locks: [{'topic': 'e_sto...
* /twist_mux/topics: [{'topic': 'joy_t...
* /use_sim_time: True
NODES
/
apriltag_ros_continuous_node (apriltag_ros/apriltag_ros_continuous_node)
apriltag_setpoint_publisher_node (mavros_apriltag_tracking/apriltag_setpoint_publisher.py)
base_controller_spawner (controller_manager/spawner)
ekf_localization (robot_localization/ekf_localization_node)
gazebo (gazebo_ros/gzserver)
gazebo_gui (gazebo_ros/gzclient)
kf_tracker_node (mavros_apriltag_tracking/kf_tracker_node)
mavros (mavros/mavros_node)
mavros_offboard_controller_node (mavros_apriltag_tracking/mavros_offboard_controller.py)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rvizvisualisation (rviz/rviz)
sitl (px4/px4)
spawn_husky_model (gazebo_ros/spawn_model)
tf_drone_to_camera (tf/static_transform_publisher)
tf_world_to_map (tf/static_transform_publisher)
tf_world_to_odom (tf/static_transform_publisher)
twist_marker_server (interactive_marker_twist_server/marker_server)
twist_mux (twist_mux/twist_mux)
vehicle_spawn_hzx_23473_2589006505986292624 (gazebo_ros/spawn_model)
/joy_teleop/
joy_node (joy/joy_node)
teleop_twist_joy (teleop_twist_joy/teleop_node)
auto-starting new master
process[master]: started with pid [23495]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to 5b4c2b1a-3870-11eb-b692-b025aa2fdbf4
process[rosout-1]: started with pid [23515]
started core service [/rosout]
process[tf_drone_to_camera-2]: started with pid [23523]
process[tf_world_to_map-3]: started with pid [23524]
process[tf_world_to_odom-4]: started with pid [23529]
process[sitl-5]: started with pid [23541]
INFO [px4] Creating symlink /home/hzx/Firmware/ROMFS/px4fmu_common -> /home/hzx/.ros/etc
0 WARNING: setRealtimeSched failed (not run as root?)
______ __ __ ___
| ___ \ \ \ / / / |
| |_/ / \ V / / /| |
| __/ / \ / /_| |
| | / /^\ \ \___ |
\_| \/ \/ |_/
px4 starting.
INFO [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
process[gazebo-6]: started with pid [23556]
process[gazebo_gui-7]: started with pid [23568]
process[vehicle_spawn_hzx_23473_2589006505986292624-8]: started with pid [23580]
process[mavros-9]: started with pid [23590]
process[apriltag_ros_continuous_node-10]: started with pid [23591]
process[mavros_offboard_controller_node-11]: started with pid [23592]
process[apriltag_setpoint_publisher_node-12]: started with pid [23598]
process[kf_tracker_node-13]: started with pid [23599]
process[base_controller_spawner-14]: started with pid [23600]
process[ekf_localization-15]: started with pid [23606]
[ INFO] [1607334079.760994122]: State buffer length corresponds to 2.000000 seconds
process[twist_marker_server-16]: started with pid [23611]
[ INFO] [1607334079.773431672]: KF is initialized.
INFO [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
INFO [simulator] Waiting for simulator to connect on TCP port 4560
[ INFO] [1607334079.780666216]: Initializing nodelet with 12 worker threads.
process[robot_state_publisher-17]: started with pid [23618]
process[twist_mux-18]: started with pid [23643]
process[joy_teleop/joy_node-19]: started with pid [23644]
process[joy_teleop/teleop_twist_joy-20]: started with pid [23654]
process[spawn_husky_model-21]: started with pid [23656]
process[rvizvisualisation-22]: started with pid [23664]
[ INFO] [1607334079.861372785]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1607334079.876526850]: [twist_marker_server] Initialized.
[ INFO] [1607334079.876606598]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1607334079.876690242]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1607334079.876785732]: GCS bridge disabled
[ INFO] [1607334079.903802339]: Plugin 3dr_radio loaded
[ INFO] [1607334079.910926832]: Plugin 3dr_radio initialized
[ INFO] [1607334079.911043301]: Plugin actuator_control loaded
[ERROR] [1607334079.923558794]: Couldn't open joystick /dev/input/ps4. Will retry every second.
[ INFO] [1607334079.927400103]: Plugin actuator_control initialized
[ INFO] [1607334079.931112074]: Plugin adsb loaded
[ INFO] [1607334079.937293520]: Loaded tag config: 0, size: 1, frame_name: tag_0
[ WARN] [1607334079.938728324]: remove_duplicates parameter not provided. Defaulting to true
[ INFO] [1607334079.943672551]: Plugin adsb initialized
[ INFO] [1607334079.943790695]: Plugin altitude loaded
[ INFO] [1607334079.947401000]: Plugin altitude initialized
[ INFO] [1607334079.947543417]: Plugin cam_imu_sync loaded
[ INFO] [1607334079.948818458]: Plugin cam_imu_sync initialized
[ INFO] [1607334079.948947395]: Plugin command loaded
[ INFO] [1607334079.968557508]: Plugin command initialized
[ INFO] [1607334079.968671187]: Plugin companion_process_status loaded
[ INFO] [1607334079.970605204]: Plugin companion_process_status initialized
[ INFO] [1607334079.970706767]: Plugin debug_value loaded
[ INFO] [1607334079.973889369]: Plugin debug_value initialized
[ INFO] [1607334079.973928612]: Plugin distance_sensor blacklisted
[ INFO] [1607334079.974019305]: Plugin esc_status loaded
[ INFO] [1607334079.974858036]: Plugin esc_status initialized
[ INFO] [1607334079.974958749]: Plugin fake_gps loaded
[ INFO] [1607334080.003255856]: Plugin fake_gps initialized
[ INFO] [1607334080.003365082]: Plugin ftp loaded
[ INFO] [1607334080.018631053]: Plugin ftp initialized
[ INFO] [1607334080.018744114]: Plugin global_position loaded
[ INFO] [1607334080.039809358]: Plugin global_position initialized
[ INFO] [1607334080.040028455]: Plugin gps_rtk loaded
[ INFO] [1607334080.046568696]: Plugin gps_rtk initialized
[ INFO] [1607334080.046896477]: Plugin gps_status loaded
[ INFO] [1607334080.051629105]: Plugin gps_status initialized
[ INFO] [1607334080.051767660]: Plugin hil loaded
[ INFO] [1607334080.065250316]: Plugin hil initialized
[ INFO] [1607334080.065361567]: Plugin home_position loaded
[ INFO] [1607334080.068708638]: Plugin home_position initialized
[ INFO] [1607334080.068831523]: Plugin imu loaded
[ INFO] [1607334080.075574413]: Plugin imu initialized
[ INFO] [1607334080.075676682]: Plugin landing_target loaded
[ INFO] [1607334080.086260870]: Plugin landing_target initialized
[ INFO] [1607334080.086366052]: Plugin local_position loaded
[ INFO] [1607334080.091531041]: Plugin local_position initialized
[ INFO] [1607334080.091638078]: Plugin log_transfer loaded
[ INFO] [1607334080.094353308]: Plugin log_transfer initialized
[ INFO] [1607334080.094462926]: Plugin manual_control loaded
[ INFO] [1607334080.096609611]: Plugin manual_control initialized
[ INFO] [1607334080.096709338]: Plugin mocap_pose_estimate loaded
[ INFO] [1607334080.099591835]: Plugin mocap_pose_estimate initialized
[ INFO] [1607334080.099710177]: Plugin mount_control loaded
[ INFO] [1607334080.104738675]: Plugin mount_control initialized
[ INFO] [1607334080.104850680]: Plugin obstacle_distance loaded
[ INFO] [1607334080.107939296]: Plugin obstacle_distance initialized
[ INFO] [1607334080.108048969]: Plugin odom loaded
[ INFO] [1607334080.111491215]: Plugin odom initialized
[ INFO] [1607334080.111595509]: Plugin onboard_computer_status loaded
[ INFO] [1607334080.113280184]: Plugin onboard_computer_status initialized
[ INFO] [1607334080.113384741]: Plugin param loaded
[ INFO] [1607334080.115412029]: Plugin param initialized
[ INFO] [1607334080.115520941]: Plugin play_tune loaded
[ INFO] [1607334080.117339164]: Plugin play_tune initialized
[ INFO] [1607334080.117444818]: Plugin px4flow loaded
[ INFO] [1607334080.123982800]: Plugin px4flow initialized
[ INFO] [1607334080.124024974]: Plugin rangefinder blacklisted
[ INFO] [1607334080.124123944]: Plugin rc_io loaded
[ INFO] [1607334080.127235141]: Plugin rc_io initialized
[ INFO] [1607334080.127352123]: Plugin safety_area blacklisted
[ INFO] [1607334080.127572580]: Plugin setpoint_accel loaded
[ INFO] [1607334080.129977560]: Plugin setpoint_accel initialized
[ INFO] [1607334080.130103427]: Plugin setpoint_attitude loaded
[ INFO] [1607334080.137644147]: Plugin setpoint_attitude initialized
[ INFO] [1607334080.137837223]: Plugin setpoint_position loaded
Gazebo multi-robot simulator, version 9.0.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
[Wrn] [GuiIface.cc:199] g/gui-plugin is really loading a SystemPlugin. To load a GUI plugin please use --gui-client-plugin
[ INFO] [1607334080.148141827]: Plugin setpoint_position initialized
[ INFO] [1607334080.148328836]: Plugin setpoint_raw loaded
Gazebo multi-robot simulator, version 9.0.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
[ INFO] [1607334080.154901573]: Plugin setpoint_raw initialized
[ INFO] [1607334080.155102396]: Plugin setpoint_trajectory loaded
[ INFO] [1607334080.158864105]: Plugin setpoint_trajectory initialized
[ INFO] [1607334080.159045264]: Plugin setpoint_velocity loaded
[ INFO] [1607334080.165273753]: Plugin setpoint_velocity initialized
[ INFO] [1607334080.165653597]: Plugin sys_status loaded
[ INFO] [1607334080.177870600]: Plugin sys_status initialized
[ INFO] [1607334080.177988484]: Plugin sys_time loaded
[ INFO] [1607334080.186308546]: TM: Timesync mode: MAVLINK
[ INFO] [1607334080.187608740]: Plugin sys_time initialized
[ INFO] [1607334080.187703304]: Plugin trajectory loaded
[ INFO] [1607334080.191616584]: Plugin trajectory initialized
[ INFO] [1607334080.191736702]: Plugin vfr_hud loaded
[ INFO] [1607334080.192368744]: Plugin vfr_hud initialized
[ INFO] [1607334080.192405768]: Plugin vibration blacklisted
[ INFO] [1607334080.192527310]: Plugin vision_pose_estimate loaded
[ INFO] [1607334080.200557712]: Plugin vision_pose_estimate initialized
[ INFO] [1607334080.200701378]: Plugin vision_speed_estimate loaded
[ INFO] [1607334080.203335572]: Plugin vision_speed_estimate initialized
[ INFO] [1607334080.203473477]: Plugin waypoint loaded
[ INFO] [1607334080.206786317]: Plugin waypoint initialized
[ INFO] [1607334080.206833058]: Plugin wheel_odometry blacklisted
[ INFO] [1607334080.206936083]: Plugin wind_estimation loaded
[ INFO] [1607334080.207382922]: Plugin wind_estimation initialized
[ INFO] [1607334080.207416000]: Autostarting mavlink via USB on PX4
[ INFO] [1607334080.207474614]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1607334080.207498042]: Built-in MAVLink package version: 2020.10.11
[ INFO] [1607334080.207522160]: Known MAVLink dialects: common ardupilotmega ASLUAV all autoquad icarous matrixpilot paparazzi standard uAvionix ualberta
[ INFO] [1607334080.207542369]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1607334080.256192908]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1607334080.257576187]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.134.255.136
[ INFO] [1607334080.279295839]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1607334080.280356474]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.134.255.136
[ INFO] [1607334080.588407580]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [1607334080.630822, 0.000000]: tag_setpoint_publisher_node is started
[ INFO] [1607334080.646105836, 1349.896000000]: Physics dynamic reconfigure ready.
[INFO] [1607334080.732152, 0.000000]: Loading model XML from file /home/hzx/Firmware/Tools/sitl_gazebo/models/typhoon_h480/typhoon_h480.sdf
[ WARN] [1607334080.742063850, 1349.992000000]: Failed to meet update rate! Took 1349.9919999999999618
[ WARN] [1607334080.743557188, 1349.992000000]: Failed to meet update rate! Took 1349.97199999999998
[INFO] [1607334080.749761, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[INFO] [1607334080.751420, 0.000000]: Calling service /gazebo/spawn_sdf_model
[ WARN] [1607334080.818238563, 1350.068000000]: Filter is not initialized. Requires a measurement first.
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
INFO [init] Mixer: etc/mixers/hexa_x.main.mix on /dev/pwm_output0
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 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 14558 remote port 14530
INFO [logger] logger started (mode=all)
INFO [logger] Start file log (type: full)
INFO [logger] Opened full log file: ./log/2020-12-07/09_41_20.ulg
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
[ INFO] [1607334080.983222837, 1350.192000000]: udp0: Remote address: 127.0.0.1:14580
[ WARN] [1607334080.983424124, 1350.192000000]: GP: No GPS fix
[ INFO] [1607334080.983535265, 1350.192000000]: FCU: [logger] file: ./log/2020-12-07/09_41_20.ulg
[ INFO] [1607334080.983600415, 1350.192000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
INFO [px4] Startup script returned successfully
pxh> INFO [mavlink] partner IP: 127.0.0.1
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
[Msg] Camera on udp port 14530
[Wrn] [msgs.cc:1852] Conversion of sensor type[sonar] not supported.
[INFO] [1607334081.280305, 1350.192000]: Spawn status: SpawnModel: Successfully spawned entity
[Wrn] [gazebo_sonar_plugin.cpp:92] [gazebo_sonar_plugin]: typhoon_h480::sonar using sonar topic "sonar"
10.80
[Wrn] [gazebo_barometer_plugin.cpp:66] [gazebo_barometer_plugin] Using default home altitude of 488 m
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [rotor_4_joint] found for channel[0] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [rotor_5_joint] found for channel[1] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [rotor_2_joint] found for channel[2] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [rotor_3_joint] found for channel[3] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [rotor_0_joint] found for channel[4] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [rotor_1_joint] found for channel[5] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [typhoon_h480::cgo3_camera_joint] found for channel[6] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [typhoon_h480::cgo3_camera_joint] found for channel[7] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [typhoon_h480::cgo3_vertical_arm_joint] found for channel[8] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [left_leg_joint] found for channel[9] joint control active for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:129] joint [right_leg_joint] found for channel[10] joint control active for this channel.
[Msg] Connecting to PX4 SITL using TCP
[Msg] Lockstep is enabled
[Msg] Speed factor set to: 1
[vehicle_spawn_hzx_23473_2589006505986292624-8] process has finished cleanly
log file: /home/hzx/.ros/log/5b4c2b1a-3870-11eb-b692-b025aa2fdbf4/vehicle_spawn_hzx_23473_2589006505986292624-8*.log
INFO [simulator] Simulator connected on TCP port 4560.
[Msg] Using MAVLink protocol v2.0
[Msg] GimbalControllerPlugin::Init
[ INFO] [1607334081.816733145, 1350.208000000]: IMU: High resolution IMU detected!
[Wrn] [msgs.cc:1852] Conversion of sensor type[sonar] not supported.
[Wrn] [msgs.cc:1852] Conversion of sensor type[sonar] not supported.
Warning [parser_urdf.cc:1115] multiple inconsistent <gravity> exists due to fixed joint reduction overwriting previous value [true] with [false].
Topic [///joint_cmd] is not valid.
[Err] [JointController.cc:72] Error subscribing to topic [///joint_cmd]
Service [///joint_cmd_req] is not valid.
[Err] [JointController.cc:79] Error advertising service [///joint_cmd_req]
[ INFO] [1607334082.362673161, 1350.396000000]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1607334082.362714316, 1350.396000000]: Starting Laser Plugin (ns = /)
[ INFO] [1607334082.363716447, 1350.396000000]: Laser Plugin (ns = /) <tf_prefix_>, set to ""
[Wrn] [ColladaLoader.cc:1805] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1805] Triangle input semantic: 'COLOR' is currently not supported
[ INFO] [1607334082.449867436, 1350.396000000]: Loading gazebo_ros_control plugin
[ INFO] [1607334082.449951606, 1350.396000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1607334082.450438245, 1350.396000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1607334082.569281906, 1350.396000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/front_left_wheel
[ERROR] [1607334082.570228058, 1350.396000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/front_right_wheel
[ERROR] [1607334082.571779355, 1350.396000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rear_left_wheel
[ERROR] [1607334082.573461469, 1350.396000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rear_right_wheel
[ INFO] [1607334082.581694440, 1350.396000000]: Loaded gazebo_ros_control.
[ INFO] [1607334082.594640942, 1350.396000000]: imu plugin missing <xyzOffset>, defaults to 0s
[ INFO] [1607334082.594713209, 1350.396000000]: imu plugin missing <rpyOffset>, defaults to 0s
[ INFO] [1607334082.627755959, 1350.396000000]: Controller state will be published at 50Hz.
[ INFO] [1607334082.629690661, 1350.396000000]: Wheel separation will be multiplied by 1.875.
[ INFO] [1607334082.633286568, 1350.396000000]: Left wheel radius will be multiplied by 1.
[ INFO] [1607334082.633393617, 1350.396000000]: Right wheel radius will be multiplied by 1.
[ INFO] [1607334082.635559101, 1350.396000000]: Velocity rolling window size of 2.
[ INFO] [1607334082.637113532, 1350.396000000]: Velocity commands will be considered old if they are older than 0.25s.
[ INFO] [1607334082.638773098, 1350.396000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1607334082.640576629, 1350.396000000]: Base frame_id set to base_link
[ INFO] [1607334082.640999241, 1350.396000000]: Odometry frame_id set to odom
[ INFO] [1607334082.641552455, 1350.396000000]: Publishing to tf is disabled
[ INFO] [1607334082.661645061, 1350.396000000]: left wheel to origin: 0.256,0.2854, 0.03282
[ INFO] [1607334082.661861147, 1350.396000000]: right wheel to origin: 0.256,-0.2854, 0.03282
[ INFO] [1607334082.662430072, 1350.396000000]: Odometry params : wheel separation 1.07025, left wheel radius 0.1651, right wheel radius 0.1651
[ INFO] [1607334082.666090635, 1350.396000000]: Adding left wheel with joint name: front_left_wheel and right wheel with joint name: front_right_wheel
[ INFO] [1607334082.666439410, 1350.396000000]: Adding left wheel with joint name: rear_left_wheel and right wheel with joint name: rear_right_wheel
[ INFO] [1607334082.697485932, 1350.396000000]: Dynamic Reconfigure:
DynamicParams:
Odometry parameters:
left wheel radius multiplier: 1
right wheel radius multiplier: 1
wheel separation multiplier: 1.875
Publication parameters:
Publish executed velocity command: disabled
Publication rate: 50
Publish frame odom on tf: disabled
[spawn_husky_model-21] process has finished cleanly
log file: /home/hzx/.ros/log/5b4c2b1a-3870-11eb-b692-b025aa2fdbf4/spawn_husky_model-21*.log
INFO [ecl/EKF] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
[ERROR] [1607334083.305969633, 1351.004000000]: ODOM: Ex: Could not find a connection between 'drone_link' and 'base_link_frd' because they are not part of the same tree.Tf has two or more unconnected trees.
[ INFO] [1607334083.307274589, 1351.004000000]: IMU: Attitude quaternion IMU detected!
[ WARN] [1607334083.370861008, 1351.068000000]: Filter is not initialized. Requires a measurement first.
INFO [ecl/EKF] 732000: EKF GPS checks passed (WGS-84 origin set)
[ INFO] [1607334083.496711334, 1351.192000000]: WP: Using MISSION_ITEM_INT
[ INFO] [1607334083.496773489, 1351.192000000]: VER: 1.1: Capabilities 0x000000000000e4ef
[ INFO] [1607334083.496804146, 1351.192000000]: VER: 1.1: Flight software: 010a0100 (e0f016c2b3000000)
[ INFO] [1607334083.496833832, 1351.192000000]: VER: 1.1: Middleware software: 010a0100 (e0f016c2b3000000)
[ INFO] [1607334083.496857016, 1351.192000000]: VER: 1.1: OS software: 040f00ff (427238133be2b0ec)
[ INFO] [1607334083.496878918, 1351.192000000]: VER: 1.1: Board hardware: 00000001
[ INFO] [1607334083.496897071, 1351.192000000]: VER: 1.1: VID/PID: 0000:0000
[ INFO] [1607334083.496914864, 1351.192000000]: VER: 1.1: UID: 4954414c44494e4f
[ WARN] [1607334083.499128166, 1351.196000000]: CMD: Unexpected command 520, result 0
[ WARN] [1607334084.382717501, 1352.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334085.385767380, 1353.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334086.390758525, 1354.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334087.393596509, 1355.068000000]: Filter is not initialized. Requires a measurement first.
INFO [ecl/EKF] 5188000: EKF commencing GPS fusion
[ WARN] [1607334088.394976527, 1356.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334089.398241279, 1357.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334090.399785909, 1358.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334091.401667666, 1359.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334092.411155233, 1360.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334093.413077636, 1361.068000000]: Filter is not initialized. Requires a measurement first.
[ INFO] [1607334094.094408165, 1361.748000000]: FCU: ARMED by Arm/Disarm component command
[ WARN] [1607334094.110179682, 1361.764000000]: CMD: Unexpected command 176, result 0
[ WARN] [1607334094.414670333, 1362.068000000]: Filter is not initialized. Requires a measurement first.
INFO [commander] Takeoff detected
[ INFO] [1607334095.374725460, 1363.012000000]: FCU: Takeoff detected
[ WARN] [1607334095.431529671, 1363.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334096.434794679, 1364.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334097.444305396, 1365.068000000]: Filter is not initialized. Requires a measurement first.
[ INFO] [1607334097.568354619, 1365.192000000]: WP: mission received
[ WARN] [1607334098.444102674, 1366.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334099.448642521, 1367.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334100.450784964, 1368.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334101.452149883, 1369.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334102.458440990, 1370.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334103.461666458, 1371.068000000]: Filter is not initialized. Requires a measurement first.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/typhoon_h480/motor_speed/4, deleting message. This warning is printed only once.
[ WARN] [1607334104.462987229, 1372.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334105.466693557, 1373.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334106.465775039, 1374.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334107.467839833, 1375.068000000]: Filter is not initialized. Requires a measurement first.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/typhoon_h480/motor_speed/5, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/typhoon_h480/motor_speed/2, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/typhoon_h480/motor_speed/3, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/typhoon_h480/motor_speed/0, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/typhoon_h480/motor_speed/1, deleting message. This warning is printed only once.
[ WARN] [1607334108.470023106, 1376.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334109.471430786, 1377.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334110.476298644, 1378.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334111.476724041, 1379.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334112.479217284, 1380.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334113.484993318, 1381.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334114.487110510, 1382.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334115.489470135, 1383.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334116.489633328, 1384.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334117.491405551, 1385.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334118.492894476, 1386.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334119.494398333, 1387.068000000]: Filter is not initialized. Requires a measurement first.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/typhoon_h480/motors, deleting message. This warning is printed only once.
[ WARN] [1607334120.507370704, 1388.068000000]: Filter is not initialized. Requires a measurement first.
[ WARN] [1607334121.508977545, 1389.068000000]: Filter is not initialized. Requires a measurement first.
[rvizvisualisation-22] killing on exit
[joy_teleop/teleop_twist_joy-20] killing on exit
[robot_state_publisher-17] killing on exit
[joy_teleop/joy_node-19] killing on exit
[twist_mux-18] killing on exit
[twist_marker_server-16] killing on exit
[ekf_localization-15] killing on exit
[base_controller_spawner-14] killing on exit
[kf_tracker_node-13] killing on exit
[apriltag_setpoint_publisher_node-12] killing on exit
[ERROR] [1607334122.451786122, 1390.008000000]: Can't accept new commands. Controller is not running.
[mavros_offboard_controller_node-11] killing on exit
[apriltag_ros_continuous_node-10] killing on exit
[mavros-9] killing on exit
[gazebo_gui-7] killing on exit
[gazebo-6] killing on exit
[sitl-5] killing on exit
[tf_world_to_odom-4] killing on exit
Exiting...
pxh> Shutting down
[tf_world_to_map-3] killing on exit
Traceback (most recent call last):
File "/home/hzx/apriltag_ws/src/mavros_apriltag_tracking/scripts/mavros_offboard_controller.py", line 550, in <module>
loop.sleep()
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 103, in sleep
sleep(self._remaining(curr_time))
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
[tf_drone_to_camera-2] killing on exit
^C[gazebo-6] escalating to SIGTERM
[gazebo_gui-7] escalating to SIGTERM
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
The nodes running on my computer are :
**rosnode list**
/apriltag_ros_continuous_node
/apriltag_setpoint_publisher_node
/base_controller_spawner
/ekf_localization
/gazebo
/gazebo_gui
/joy_teleop/joy_node
/joy_teleop/teleop_twist_joy
/kf_tracker_node
/mavros
/mavros_offboard_controller_node
/robot_state_publisher
/rosout
/rvizvisualisation
/tf_drone_to_camera
/tf_world_to_map
/tf_world_to_odom
/twist_marker_server
/twist_mux
@zsyfight Do you still have this issue?
Can you try to clean the PX4 Firmware build, using make distclean and then re-build again using DONT_RUN=1 make px4_sitl gazebo
@zsyfight I was able to re-produce your issue. I think this issue happens after updating PX4 to v1.11.2. I solved this issue by
- clean and re-build PX4 Firmware (v1.11.2) as mentioned above
- I updated the typhoon sdf model in this package. Re-clone the folder in this package
models/typhoon_h480to~/Firmware/Tools/sitl_gazebo/models - Run the simulation again.
Hi @mzahana, i have the same issue, and my px4 version is v1.11.0, i do what you say above make distclean and then DONT_RUN=1 make px4_sitl gazebo Re-clone the folder in this package models/typhoon_h480 to ~/Firmware/Tools/sitl_gazebo/models Run the simulation again then the same errors are shown on the terminal how can i fix it? Thanks in advance
I have the same issue too, i figured out this trouble by fixxing some bugs in "/src/kf_tracker.cpp".
The original program uses the Eigen library for matrix operations.
In function (line 165) KFTracker::predict(void), kf_state_pred_.x = F_kf_state_pred_.x , F_ is a 66 matrix,
but kf_state_pred_.x has changed its dimension by line 286, now it becomes a 3-dimensional vector.
To figure out this bug, I delete the line 286, and use
kf_state_pred_.x(0,0)= z_meas_.z(0) ;
kf_state_pred_.x(1,0)= z_meas_.z(1) ;
kf_state_pred_.x(2,0)= z_meas_.z(2) ;
to replace.
Hi @mzahana, i have the same issue too. I try all above,but it doesn't work. how can i fix it? Thanks in advance
